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Abstract 

In  this  paper,  we  propose  a  new  approach  for  planning  the  motion  of  robotic  systems  among  obstacles, 
which  is  based  on  a  distributed  representation  of  the  world  model.  Within  this  approach,  we  designed  and 
implemented  a  general  purpose  path  planner  with  hve  new  capabilities: 

(1)  It  is  able  to  generate  very  complex  motions  for  robots  with  many  degrees  of  freedom.  In  particular, 

we  succeeded  in  generating  complex  paths  for  a  10  DOF  non-serial  manipulator  arm  made  with  both 
revolute  and  prismatic  joints. 

(2)  It  is  drastically  faster  (between  1  and  2  orders  of  magnitude)  than  existing  systems  on  a  sequential 

computer.  We  generated  complex  paths  for  a  3  DOF  bar  in  a  2D  workspace  in  about  1  second  on  a 
MIPS-based  workstation,  as  opposed  to  minutes  or  even  tens  of  minutes  for  other  algorithms. 

(3)  The  algorithms  are  highly  parallelizable.  We  envision  a  VLSI  implementation  which  should  allow  to 

generate  plans  in  real-time,  even  for  10  DOF  arms.  Real-time  motion  planning  for  complex  robotics 
systems  opens  new  perspectives  on  some  key  issues  such  as  motion  planning  in  incompletely  known 
or  unknown  environment,  motion  planning  among  fast  moving  obstacles  and  multi-robot  motion 
planning. 

(4)  Our  approach  extends  to  motion  planning  problems  with  non-integrable  constraints,  e.g.  non-holonomic 

and/or  dynamic  constraints.  In  particular,  the  planner  was  able  to  deal  systematically  with  maneuvers 
for  a  non-holonomic  car. 

(5)  The  planner  outputs  a  path  for  the  robot  in  configuration  space,  while  the  goal  is  specified  in  opera¬ 

tional  space.  Hence,  the  inverse  kinematic  problem  is  completely  avoided.  Furthermore,  any  kind  of 
redundancy  of  the  robot  arms  can  be  handled  without  modification. 

These  capabilities  are  obtained  through  the  systematic  use  of  low-level  distributed  multiscale  represen¬ 
tations  of  the  world.  These  representations  allow  to  apply  simph  powerful  numerical  techniques  to 
geometrical  and  physical  concepts. 

The  approacii  is  illustrated  by  several  simulation  examples,  bot:.  i.  -th  mobile  robots  and  manipulator 
arms. 


ACKNOWLEDGMENTS.  This  research  was  funded  by  DARPA  contract  DAAA21-89-C00Q2  (Army), 
DARPA  contract  N00014-8S-K-0620  (Office  of  Naval  Research),  SIMA  (Stanford  Institute  of  Manufacturing 
and  .Automation),  CIFE  (Center  for  Integrated  Facility  Engineering),  and  a  Digital  Equipment  Corporation 
grant. 

Tlic  autl'ors  also  thank  Professor  M.  Harrison  from  the  Stanford  University  Graduate  Business  School 
and  J.  Cheng  from  the  Stanford  University  Statistical  Consulting  Service  for  their  sound  advice  on  Brownian 
motions  witli  reflective  boundaries. 


1 


1  Introduction 


In  this  paper,  we  propose  a  new  approach  for  planning  the  motion  of  robotic  systems  among 
obstacles,  which  is  baised  on  a  distributed  representation  of  the  world  model.  Within  this 
approach,  we  designed  and  implemented  a  general  purpose  path  planner  with  five  new  ca¬ 
pabilities; 

(1)  It  is  able  to  generate  very  complex  motions  for  robots  with  many  degrees  of  freedom. 

In  particular,  we  succeeded  in  generating  complex  paths  for  a  10  DOF  non-serial  ma¬ 
nipulator  arm  made  with  both  revolute  and  prismatic  joints. 

(2)  It  is  drastically  faster  (between  1  and  2  orders  of  magnitude)  than  existing  systems  on  a 

sequential  computer.  We  generated  complex  paths  for  a  3  DOF  bar  in  a  2D  workspace 
in  about  1  second  on  a  MIPS-based  workstation,  ais  opposed  to  minutes  or  even  tens 
of  minutes  for  other  algorithms. 

(3)  The  zdgorithms  are  highly  parallelizable.  We  envision  a  VLSI  implementation  which 

should  allow  to  generate  plans  in  real-time,  even  for  10  DOF  arms.  Real-time  motion 
planning  for  complex  robotics  systems  opens  new  perspectives  on  some  key  issues  such 
as  motion  planning  in  incompletely  known  or  unknown  environment,  motion  planning 
among  fast  moving  obstacles  and  multi-robot  motion  planning. 

(4)  Our  approach  extends  to  motion  planning  problems  with  non-integrable  constraints,  e.g. 

non-holonomic  and/or  dynamic  constraints.  In  particular,  the  planner  was  able  to  deal 
systematically  with  maneuvers  for  a  non-holonomic  car. 

(5)  The  planner  outputs  a  path  for  the  robot  in  configuration  space,  while  the  goal  is 

specified  in  operational  space.  Hence,  the  inverse  kinematic  problem  is  completely 
avoided.  Furthermore,  any  kind  of  redundancy  of  the  robot  arms  can  be  handled 
without  modification. 

These  capabilities  are  obtained  through  the  systematic  use  of  a  low-level  distributed 
multiscale  representation  of  the  world.  This  representation  allows  us  to  apply  simple  and 
powerful  numerical  techniques  to  geometrical  and  physical  concepts.  More  specifically,  we 
compute  numerical  potential  fields  in  configuration  space  that  have  very  few  or  very  small 
local  minima,  and  we  provide  the  planner  with  a  way  to  escape  systematically  from  these 
minima.  The  approach  is  illustrated  by  several  simulation  examples,  both  with  mobile  robots 
and  manipulator  arms. 

The  principle  of  our  approach  is  to  throw  attractive  potential  fields  over  the  workspace, 
each  applying  to  a  specific  point  on  the  robot  body,  and  then  to  combine  these  potentials  in 
configuration  space  to  attract  the  whole  robot  toward  the  desired  goal.  Each  of  the  workspace 
potential  fields  is  computed  numerically  and  has  no  other  local  minimum  than  the  goal  of 
the  robot’s  point  it  acts  on.  Basically,  these  potentials  allow  to  avoid  all  the  workspace 
concavities.  However,  when  these  potentials  are  applied  concurrently  at  the  different  points 
of  the  robot,  the  resulting  potential  in  configuration  space  may  have  (and  indeed  has)  local 
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minima  other  than  the  goal.  However,  these  minima  generally  determine  wells  of  relatively 
small  depth.  The  idea  then  is  to  build  a  graph  connecting  the  local  minima  and  perform  a 
search  of  this  graph  until  the  goal  is  attained.  The  search  of  the  local  minima  graph  can  be 
implemented  in  several  ways. 

For  example,  [Barra  89]  presents  an  approach  that  connects  the  different  local  minima  by 
tracking  the  valleys  of  the  potential.  This  approach  worked  pretty  well  for  a  small  number  of 
DOF,  but  appeared  unable  -  at  least  with  the  current  implementation  -  to  deal  with  some 
complex  motions  for  a  large  number  of  DOF,  although  it  succeded  in  some  czises. 

In  this  paper,  we  present  two  new  approaches  for  building  and  searching  this  local  minima 
graph.  The  first  consists  of  a  simple  brute  force  exploration  of  the  local  minima  in  the 
discretized  configuration  space,  which  happens  to  be  very  efficient  when  applied  to  robots 
with  a  small  number  of  DOF.  The  second  is  baised  on  a  Monte-Carlo  procedure.  It  is 
much  more  general  (it  has  solved  tricky  path  planning  problems  for  a  10  DOF  robot),  and 
furthermore  highly  parallelizable.  We  strongly  believe  that  this  second  approach  is  definitely 
the  best  when  massively  parallel  hardware  becomes  commonly  available.  However,  the  first 
method  is  faster  for  small  robots  on  a  sequential  computer  and  is  deterministically  resolution- 
complete^  whereas  the  Monte-Carlo  approach  is  only  probabilistically  resolution-complete. 

We  model  the  obstacles  as  distributed  bitmap  descriptions  rather  than  centralized  semi- 
algebraic  descriptions  as  it  is  usually  the  case  in  the  literature  on  path  planning.  We  think 
that  distributed  descriptions  axe  simpler  to  obtain  from  sensory  data  axid  that  they  may 
be  easier  to  handle  algorithmically  (specially  when  massively  parallel  computing  hardware 
becomes  commonly  available).  In  particular,  the  ‘perfect  potential  fields’  computed  numer¬ 
ically  in  our  approach  could  not  in  general  be  computed  analytically  with  closed  mathe¬ 
matical  functions.  In  addition,  using  bitmap  descriptions  opens  new  perspectives  on  some 
computational  complexity  issues.  While  the  complexity  of  path  planning  algorithms  using 
semi-algebraic  models  is  usually  polynomial  in  the  number  of  algebraic  constraints  and  in 
the  maximal  degree  of  these  constraints,  the  complexity  of  our  algorithm  is  polynomial  in 
the  inverse  of  the  resolution  of  the  bitmap  descriptions*. 

We  have  implemented  our  approach  and  experimented  with  it  on  many  examples,  simu¬ 
lating  both  mobile  robots  and  manipulator  arms.  Some  of  the  most  significant  experiments 
are  reported  in  this  paper.  In  particular,  for  a  3  DOF  bar  amidst  a  planar  maze  composed 
of  more  than  70  complex-shaped  obstacles,  we  have  succeeded  in  generating  paths  in  about 
5  second  on  a  DEC-3100  workstation.  We  have  also  been  able  to  generate  complex  paths 
including  several  maneuvers  for  a  non-holonomic  3  DOF  planar  mobile  robot  (a  car  with 
limited  steering  angle)  in  about  1  minute.  To  our  knowledge,  our  planner  is  the  first  one 
capable  of  solving  systematically  non-holonomic  motion  planning  problems.  Finally,  and 
most  importantly,  we  have  succeeded  in  generating  complex  paths  for  a  non-serial  lO-DOF 
manipulator  arm  including  both  prismatic  and  revolute  joints.  All  the  examples  presented 

'  Different  algorithm  complexity  measures  with  non  algebraic  models  have  already  been  given  in 
[Lumelsky  87], 
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so  far  to  the  planner  have  been  solved  within  a  few  minutes  of  computation.  However,  as  the 
Monte- Carlo  procedure  is  not  deterministically  complete,  and  as  the  path  planning  problem 
is  known  to  be  NP-Hard,  it  is  certainly  possible  to  find  problems  which  the  algorithm  would 
fail  to  solve  in  a  reasonable  amount  of  time.  We  are  currently  investigating  this  issue.  To 
our  knowledge,  our  planner  is  the  first  one  capable  of  solving  systematically  motion  planning 
problems  of  this  complexity. 

As  the  Monte-Carlo  procedure  is  easily  parallelizable,  we  envision  a  VLSI  implementation 
of  the  algorithm  for  10  DOF  manipulator  arms,  which  should  allow  to  generate  plans  in  real¬ 
time.  If  motion  planning  can  be  performed  in  real-time,  it  can  be  included  in  closed  loop  in 
control  algorithms.  The  concept  of ‘reactive  planning’  then  reduces  to  closed-loop  real-time 
motion  planning.  Therefore,  real-time  motion  planning  for  complex  robotics  systems  opens 
new  perspectives  on  some  key  issues  in  robotics  such  as  motion  planning  in  incompletely 
known  or  unknown  environment,  motion  planning  among  fast  moving  obstacles,  multi-robot 
motion  planning. 

The  paper  is  organized  as  follows.  In  Section  3,  we  describe  how  the  workspace  numerical 
potential  fields  are  computed  and  how  a  generic  robot  interacts  with  these  potentials.  In 
Section  4,  we  detail  the  very  fast  hierarchical  graph  search  planner  implemented  for  3  DOF, 
and  we  show  some  experimental  results  obtained  with  a  long  bar  in  different  2D  workspaces. 
We  also  compare  these  results  with  those  computed  using  the  Monte-Carlo  procedure.  In 
Section  5,  we  describe  the  application  of  the  technique  to  non-holonomic  constraints.  We 
show  some  experimental  results  where  a  car  with  limited  steering  angle  plans  complex  ma¬ 
neuvers.  In  Section  6,  we  give  a  detailed  account  of  the  Monte-Carlo  approach  which  allowed 
to  solve  complex  planning  problems  for  complex  robots  (in  particular  for  the  10  DOF  non¬ 
serial  manipulator  arm)  and  show  corresponding  simulation  results.  Finally,  we  discuss  in 
Section  7  the  parallelization  issues  relative  to  the  different  steps  of  the  motion  planning  al¬ 
gorithm.  All  the  experiments  reported  below  were  carried  out  on  a  MIPS-based  DEC-3100 
workstation. 


2  Relation  to  other  work 

Research  on  motion  planning  has  been  quite  intensive  during  the  past  ten  years.  Most  of 
it  has  focused  on  path  planning,  i.e.  the  topological  and  geometrical  problem  of  finding 
a  collision-free  path  between  two  given  configurations  of  a  robot,  or  more  generally  be¬ 
tween  two  given  subsets  of  configuration  space.  Today,  the  mathematical  and  computational 
structures  of  the  general  problem  (when  stated  in  algebraic  terms)  is  reasonably  well  un¬ 
derstood  ([Schwartz  S2,Schwartz  S7a,Schwartz  S7b, Canny  87]).  In  addition,  some  practical 
algorithms  have  been  implemented  in  more  or  less  specific  cases  (e.g.,  [Brooks  83,Fav  84, 
Gonzenes  84, Lozano  87]). 

One  of  the  most  extensively  studied  path  planning  approach  is  the  cell  decomposition 
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approach.  It  consists  of  first  decomposing  (exactly  or  approximately)  the  set  of  free  configu¬ 
rations  of  the  robot  into  a  finite  collection  of  cells  (e.g.,  Collins  cells  for  exact  decomposition 
and  rectangular  cells  for  approximate  decomposition),  and  then  searching  a  graph  represent¬ 
ing  the  connectivity  between  these  cells.  However,  in  this  approach,  the  number  of  cells  to 
be  generated  is  a  function  of  the  number  of  semi-algebraic  constraints  used  to  model  the 
robot  and  the  obstacles,  and  of  the  degree  of  these  constraints.  This  function  tends  to  grow 
exponentially  with  the  number  n  of  DOF,  as  the  volume  of  the  configuration  space  (locally 
diffeomorphic  to  R")  increases  exponentially  with  n.  Thus,  the  approach  is  untractable  even 
for  reasonably  small  values  of  n.  To  our  knowledge,  no  planner  has  been  implemented  using 
this  approach  with  n  >  4.  In  fact,  this  is  true  of  other  so-called  ‘global’  methods,  which 
represent  the  connectivity  of  free  space  as  a  graph  before  actually  starting  the  search  for  a 
path. 

In  an  attempt  to  cope  with  more  DOF,  an  approach,  which  consists  of  approximating 
free  space  at  successive  levels  of  detail  as  a  collection  of  rectangultir  cells  of  decreasing  size, 
has  been  developed  by  several  researchers  (see  [Brooks  83,Fav  84]).  Several  implementations 
of  this  approach  show  that  it  can  significantly  improve  the  average  computing  time  for 
small  numbers  of  DOF  (typically,  n  <  4).  However,  although  the  full  potential  of  the 
paradigm  has  probably  not  been  completely  exploited  yet,  current  results  lead  to  think  that 
it  is  not  powerful  enough  to  extend  the  applicability  of  the  cell  decomposition  approach  to 
significantly  higher  values  of  n. 

Since  the  untractability  of  the  cell  decomposition  approach  -  and  more  generally  of  the 
other  global  methods  -  is  due  in  part  to  the  precomputation  performed  before  the  search  of 
the  connectivity  graph,  local  methods  to  path  planning  have  been  considered  for  handling 
more  DOF  and  some  successful  systems  have  been  implemented  (e.g.,  [Donald  84,Fav  87]). 
A  local  path  planning  method  consists  of  placing  a  regular  grid  (at  some  resolution)  onto 
the  robot  configuration  space  and  searching  this  grid  (see  [Donald  87]).  Heuristics  computed 
from  partial  information  about  the  geometry  of  the  configuration  space  are  used  to  guide  the 
search.  Thus,  unlike  global  methods  developed  so  far,  a  local  method  requires  no  expensive 
precomputation  step  before  starting  the  search  of  a  path.  Consequently,  in  favorable  cases, 
it  runs  substantially  faster  than  any  global  method.  But,  since  the  search  graph  (i.e.,  the 
grid)  is  considerably  larger  than  the  connectivity  graph  searched  by  global  methods,  in  less 
favorable  cases,  it  may  require  much  more  time  than  global  methods.  In  order  to  deal 
with  this  difficulty,  powerful  heuristics  are  needed  to  guide  the  search,  but  known  such 
heuristics  have  the  drawback  of  eventually  guiding  the  search  into  dead-ends,  from  which  it 
is  very  difficult  to  escape.  For  example,  a  widely  used  heuristic  technique  consists  of  guiding 
the  robot  along  the  gradient  direction  generated  by  an  artificial  potential  field  [Khatib  S6]. 
It  is  well-known  that  this  technique  eventually  leads  the  search  to  local  minima  of  the 
potential  and  provides  no  way  to  escape  these  minima.  The  issue  of  defining  a  potential  field 
with  a  unique  minimum  at  the  goal  configuration  and  whose  attraction  domain  includes 
all  free  space  has  been  investigated  with  very  limited  success  so  far  ([Kod  S7,Rimon  SSj). 
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Furthermore,  if  such  a  nice  potential  could  be  defined,  its  computation  would  probably  be 
expensive  and  constitute  a  precomputation  step  before  search  similar  in  drawback  to  cell 
decomposition.  Paths  for  a  8  DOF  manipulator  have  been  generated  with  a  variant  of  the 
potential  field  method  [Fav  87].  Although  impressive,  the  results  have  been  obtained  in  a 
specific  workspace  made  of  vertical  cylindrical  pipes.  Such  a  workspace  makes  probably 
easier  the  definition  of  a  potential  field  with  few  local  minima.  Nevertheless,  the  planner 
also  required  some  human  interaction  for  escaping  local  minima. 

Recently,  we  have  developed  an  approach  based  on  a  numerical  valley  tracking  algorithm 
[Barra  89]  which  was  able  to  plan  paths  for  a  10  DOF  manipulator  arm  with  a  complex 
kinematic  chain  (the  same  arm  is  used  in  this  paper).  However,  the  approach  was  not  very 
reliable  -  it  failed  on  several  problems  -  and  quite  slow.  In  fact,  the  approach  reported  below 
derives  from  that  described  in  [Barra  89]. 

3  Distributed  Representation  and  Potential  Fields 

3.1  Overview 

Let  A  denote  the  robot  body,  W  its  workspace,  and  C  its  configuration  space  parameteriza¬ 
tion  (i.e.  generalized  coordinates).  A  configuration  of  the  robot,  i.e.  a  point  in  C,  completely 
specifies  the  position  of  every  point  in  A  with  respect  to  a  coordinate  system  attached  to 
W.  The  subset  of  C  consisting  of  all  the  configurations  where  the  robot  has  no  contact  or 
intersection  with  the  obstacles  in  W  is  called  free  space  and  denoted  Cfree- 

The  workspace  W  is  modeled  as  a  multiscale  pyramid  of  bitmap  arrays,  each  of  these 
arrays  being  2-  or  3-dimensional.  At  a  given  resolution  level,  the  array  is  represented  by  a 
function; 


BM  {1,0} 

X  ►  BM{x) 

in  such  a  way  that  the  subset  of  points  i  such  that  BM{x)  =  1  represents  the  workspace 
obstacles  and  the  subset  of  points  x  such  that  BM{x)  =  0  represents  the  empty  part  of 
the  workspace.  All  our  experiments  were  conducted  in  a  2D  workspace,  but  the  approach 
extends  without  any  change  two  a  3D  workspace  (at  the  e.xpense  of  more  computing  time). 

The  coarsest  level  of  the  bitmap  pyramid  is  typically  16^,  the  finest  256^  or  512^, 

The  bitmap  representation  of  a  particular  workspace  at  the  256^  resolution  is  shown  in 
figure  1(1=  black;  0  =  white). 

For  each  point  p  in  A,  one  can  consider  the  geometrical  relation  which  maps  the  configu¬ 
rations  7  =  (r/,. , . .  ,  r/„)  of  the  robot  to  the  position  i  of  p  in  the  workspace.  This  geometrical 
map: 
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X  -.  X  C  ^  W 


(p,?)  X{p,q)  =  X 

is  called  forward  kinematic  relation. 

Our  approach  to  path  planning  is  based  on  the  definition  of  a  numerical  potential  field 
U{q)  over  configuration  space  that  has  few  and  small  local  minima.  In  the  case  where  some 
local  minima  appear,  the  algorithm  backtracks  in  configuration  spaoe  until  the  robot  gets  out 
of  the  local  minimum.  All  the  computations,  in  configuration  spaw:e  as  well  as  in  workspace, 
are  performed  within  a  multiscale  hierarchical  data  representation.  For  each  resolution  level 
of  the  hierarchy,  the  overall  configuration  space  potential  field  U{q)  is  computed  as  a  non¬ 
linear  combination  U (q)  =  G(Up^  {X{pi,q)), . . . ,  Up^{X{pic,  q)))  of  several  workspace  potential 
fields  Upj  (x), . . . ,  Up^{x),  each  of  these  attraM:ting  concurrently  a  given  point  p,  of  the  robot 
towards  its  goal  position.  In  the  next  two  subsections,  we  describe  the  computation  of  these 
workspace  potential  fields.  Then,  in  subsection  3.4,  with  describe  the  interaction  of  the  robot 
with  these  workspace  potentials,  and  specify  the  non-linear  combination  G. 

3.2  Distance  Function  and  Voronoi  Diagram 

At  each  resolution  level  of  the  hierarchy,  the  potential  field  used  in  our  implementation  is 
constructed  in  two  steps.  In  the  first  step,  we  compute  jui  approximation  to  the  distance 
to  the  obstacles  in  the  workspace,  a.s  mentioned  in  [Barra  89].  We  build  at  the  same  time 
an  approximation  of  the  generalized  Voronoi  Diagram.  This  first  step  is  described  in  this 
subsection.  In  the  second  step,  we  compute  a  potential  field  using  this  distance  information 
and  the  Voronoi  diagram.  This  second  step  is  described  in  the  next  subsection. 

We  first  compute  the  map  di,  which  is  an  approximation  of  the  distance  to  the 
obstacles.  The  function  di  is  simply  computed  as  follows:  First  the  points  on  the  boundary 
of  {i  /  BM{x)}  are  identified  and  the  value  of  di  at  these  points  is  set  to  zero  (we  also  include 
the  points  on  the  rectangular  frame  bounding  the  bitmap  ais  boundary  points).  Then,  the 
value  of  di  at  all  the  neighbors  of  these  boundary  points  .  n  the  empty  part  of  the  workspace 
are  set  to  1;  the  value  of  dj  at  the  neighbors  of  these  points,  if  not  yet  computed,  is  set  to 
2;  etc.  The  procedure  is  recursively  repeated  until  all  the  empty  part  of  the  workspace  is 
completely  explored.  Figure  2  displays  the  equipotentials  of  di  for  the  workspace  shown  at 
figure  1.  The  valleys  of  the  map  thus  obtained,  i.e.  the  set  of  points  where  the  waves  coming 
from  the  boundary  points  meet,  form  the  discretized  Voronoi  diagram  for  the  distance. 

As  a  matter  of  fact,  we  can  define  the  generalized  Voronoi  diagram  associated  with  any 
distance  function  in  the  following  way:  For  each  point  z  in  the  free  space,  we  can  compute 
along  with  the  distance  dj  to  the  nearest  obstacle  the  set  P(z)  of  boundary  points  from 
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Figure  3:  A  Generalized  Voronoi  Diagram. 


which  this  distance  has  been  obtained.  This  associated  set  of  boundary  points  F{x)  is  the 
set  of  points  which  realize  the  minimum  distance  di(x).  The  generalized  Voronoi  diagram  is 
precisely  the  set  of  points  for  which  P{x)  contains  more  than  one  boundary  point.  Figure  3 
displays  the  generalized  Voronoi  diagram  computed  from  the  workspace  shown  on  Figure  1. 

The  computation  of  dy  is  not  local  and  therefore  must  be  done  prior  to  the  execution  of  the 
rest  of  the  path  planning  algorithm.  However,  the  procedure  is  linear  in  the  number  of  points 
of  the  bitmap  representing  W  and  is  quite  fast  (a  fraction  of  a  second  for  a  256^  bitmap). 
The  time  complexity  of  the  procedure  does  not  depend  on  the  shape  of  the  obstacles  (the 
complexity  of  computing  analytically  the  Voronoi  diagram  of  a  polygonal  workspace  increases 
with  the  number  of  vertices). 

3.3  Workspace  Potential  Fields  Without  Local  Minima 

The  idea  is  to  define  an  attractive  potential  field  [Khatib  86]  attracting  some  points  of 
the  robot  A  toward  their  goal  position.  We  could  use  a  classical  analytical  potential  field 
de[)eading  on  the  Euclidean  distance  in  W.  But  it  is  known  that  this  kind  of  field  can 
have  other  local  minima  than  the  goal,  especially  in  the  workspace  obstacle  concavities. 
The  bitmap  description  of  the  workspace  allows  us  to  compute  numerically  an  attractive 
potential  field  which  has  no  other  local  minima  than  the  goal,  when  used  on  a  point  robot. 
This  potential  happens  to  be  very  helpful  for  avoiding  concavities  of  the  workspace  obstacles. 
Let  us  consider  a  point  robot  x  moving  freely  in  the  workspace.  Let  xqoal  be  the  goal 


Figure  4:  Numerical  potential  field  t/yy  for  a  2D  workspace. 


position  of  x  in  W.  We  set  the  value  of  the  potential  at  the  goal  point  to  zero.  Then  the 
neighbors  of  xgoal  set  to  1  .  This  procedure  is  recursively  repeated  until  the  empty 
part  of  W  is  completely  explored.  The  complexity  is  linear  in  the  number  of  points  of  the 
bitmap  description,  and  constant  in  the  number  and  shape  of  obstacles.  The  equipotentials 
of  the  resulting  workspace  potential  field  Uy\>{x,  xgoal)  for  the  2D  workspace  of  figure  1  are 
displayed  in  figure  4.  This  computation  was  performed  in  a  fraction  of  a  second. 

A  property  of  this  function  is  the  following:  when  one  follows  the  gradient  from  any 
starting  point  x/a^/j,  a  path  connecting  to  xgoal  is  obtained,  which  is  the  shortest 

geometrical  path  for  the  distance.  In  a  3D  space,  this  may  be  an  important  advantage 
over  exact  shortest  distance  algorithms,  since  the  problem  of  computing  the  exact  shortest 
distance  in  a  3D  polyhedral  space  is  NP-hard  in  the  number  of  vertices  under  any  L'’  metric 
[Canny  87]. 

The  above  technique  has  interesting  extensions.  For  example,  it  is  possible  to  modify  the 
definition  of  the  potential  in  order  to  take  safety  considerations  into  account. 

We  detail  in  the  next  paragraph  an  improvement  of  the  technique  previously  presented 
in  [Barra  89]  for  building  ‘safe’  potential  fields.  The  idea  is  to  compute  first  the  distance  dj 
and  the  Voronoi  d  -^ram  Vq  as  explained  above.  Then,  the  potential  field  is  computed  in 
three  steps.  Th  Id  ‘  step  consists  of  tracing  a  line  L  following  the  gradient  of  the  distance 
d\  from  the  goai  point  xgoal  to  the  nearest  point  on  the  Vq.  Once  this  line  is  computed, 
we  obtain  a  new  se'  "  points  5  =  U  L.  This  set  5  is  connected.  The  second  step  of 
the  algorithm  s  to  label  all  the  points  of  S  starting  from  xgoal-  The  label  0  is  cissigned 
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Figure  5;  Another  numerical  potential  field  taking  safety  considerations  into  account. 


to  xcoALi  the  label  1  is  cissigned  to  its  neighbor  which  is  at  maximum  distance  di  of 
the  obstacles.  Then  we  consider  together  the  neighbors  of  xooal  a^id  ii  which  have  not 
been  labeled  yet,  and  give  the  label  2  to  the  one  which  is  the  farthest  from  the  obstacles. 
This  operation  is  recursively  repeated  until  the  set  5  =  V/j  U  L  is  completely  explored.  At 
each  step  of  the  recursion,  the  list  of  points  in  S  to  be  labeled  is  represented  with  a  heap 
structure,  so  that  each  insertion  of  a  new  point  or  extraction  of  the  maximum  distance  point 
is  performed  in  logarithmic  time.  At  the  end  of  the  second  step,  all  the  points  z  6  5  have  a 
label  l{x).  The  third  step  is  simpler,  and  very  similar  to  the  computation  of  di.  We  compute 
all  the  neighbors  of  all  the  points  in  5,  and  give  to  the  neighbor  of  i  €  5  the  label  l{x)  +  1. 
We  then  compute  the  neighbors  of  these  neighbors,  and  increment  the  labels  iteratively  until 
the  freespace  is  completely  explored. 

Instead  of  incrementing  the  labels  systematically  of  the  same  constant  1,  more  complex 
incrementation  functions  giving  smoother  potentials  can  be  easily  derived.  For  example,  we 
can  make  the  increments  in  the  third  step  dependent  of  the  distance  dj.  Instead  of  giving 
the  increment  1  to  the  neighbors  of  5,  we  could  give  the  increment  l/di.  Then,  we  could 
give  the  increment  l/di  again  to  the  neighbors  of  these  neighbors,  and  so  on.  In  that  case, 
we  would  obtain  a  potential  field  that  becomes  infinite  on  the  obstacles  boundaries.  We  will 
not  detail  further  the  cosmetics  of  numerical  potential  fields  computations,  but  we  want  to 
point  out  that  a  large  familly  of  potential  fields  with  very  nice  properties  can  be  build  within 
this  bitmap  numerical  framework. 

Figure  b  shows  the  equipotentials  of  this  last  potential  field.  The  path  obtained  by 
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tracking  its  gradient  stays  away  from  the  obstacles  (in  fact  a  point  robot  tracking  this 
potential  would  follow  the  safest  path  on  the  Voronoi  diagram.)-  However,  the  complexity 
of  computing  this  modified  potential  is  slightly  higher  than  that  of  the  previous  one.  Let 
m  be  the  number  of  points  in  the  workspace  bitmap,  and  p  the  number  of  points  lying  in 
Vq  U  L.  The  complexity  is  O(m  +  plogp)  instead  of  0{m)  (the  logarithmic  term  comes  from 
the  calculation  of  the  maximum  distance  point  in  the  Voronoi  list  with  the  heap  structure). 
For  most  reasonable  workspaces,  we  have  p  oc  m~  because  the  Voronoi  diagram  is  of 
dimension  n  —  1  in  a  n-dimensional  workspace.  Therefore,  we  have  an  average  complexity 
of  0(m  -t  m^logm),  which  simply  equals  0(Tn).  The  algorithm  is  still  linear  in  the 
average  case.  For  a  256^  workspace,  the  computation  took  about  2  seconds  (including  the 
computation  of  di  and  Vo)- 

3.4  Configuration  Space  Potential  Field 

As  the  workspace  representation  is  distributed,  the  natural  representation  for  the  robot 
should  also  be  distributed.  That  is  to  say,  the  robot  should  be  considered  as  a  the  discretized 
collection  of  all  the  points  on  A.  On  a  parallel  hardware,  this  would  be  the  simplest  and 
most  efficient  fashion  to  compute  the  distance  of  the  robot  to  the  obstacles  and  the  attractive 
potentials.  One  could  use  one  processor  per  discretized  point  on  the  robot.  However,  as  we 
made  all  our  simulations  on  a  sequential  computer,  we  slightly  adapted  this  representation 
in  order  to  reduce  the  amount  of  computations. 

First,  we  only  need  to  consider  the  points  on  the  boundary  of  the  robot,  because  the 
minimum  distance  from  the  obstacles  to  the  robot  is  always  reached  on  the  boundary  of 
the  robot.  Second,  using  the  distance  information,  we  can  adjust  adaptively  the  number  of 
points  on  the  robot  (we  call  them  control  points)  with  regards  to  the  distance  to  the  obstacles 
computed  for  each  of  these  points.  To  illustrate  this  idea,  let  us  tissume  that  we  want  to 
compute  the  minimum  distance  D\  from  the  robot  to  the  obstacles.  It  is  mathematically 
defined  as: 

Di{q)  =  ml  di{X{p,q)). 

p^A 

Let  us  consider  an  ideal  robot  composed  of  a  single  straight  line  segment  of  length  L. 
We  suppose  that  the  workspace  (unction  di  has  been  precomputed.  Instead  of  computing 
the  distance  di  for  each  discretized  point  on  the  segment  and  then  calculating  the  minimum 
of  all  these  distances,  we  choose  to  compute  first  the  distances  di(begin)  and  di{end)  of  the 
two  extremity  points  of  the  segment.  If  DDi  =  min(di(6egm),  di(end))  >  L,  then  we  are 
sure  that  the  robot  cannot  hit  the  obstacles,  and  we  choose  DD\  as  an  approximation  of 
Dp  Otherwise,  we  compute  the  distance  di  for  the  middle  point  of  the  segment.  Our  initial 
segment  can  be  considered  as  the  union  of  two  half-length  segments,  and  we  can  recursively 
apply  the  approximation  procedure  to  these  sub-segments. 

If  the  robot  boundary  can  be  modeled  by  a  polygon,  this  segment  procedure  can  be 
applied  to  each  segment  of  the  polygon.  Moreover,  the  segment  procedure  can  be  easily 
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generalized  to  higher  order  models  of  the  robot  boundary  like  arcs  of  circles,  arcs  of  ellipses, 
and  so  on.  When  the  robot  is  far  from  the  obstacles,  very  few  points  on  its  boundary  need 
to  be  checked.  When  the  robot  is  in  contact  space  however,  the  segment  which  achieves  the 
contact  has  to  be  checked  in  its  entirety. 

After  this  procedure,  we  obtain  an  approximation  of  the  distance  Di  between  the  robot 
and  the  obstacles. 

As  our  robot  can  be  a  complex  articulated  system,  we  might  need  to  check  its  auto¬ 
intersections.  For  example,  if  the  robot  is  a  manipulator  like  those  described  in  section  6.5, 
we  have  to  check  intersections  of  several  straight  line  segments  in  the  workspace.  Usually, 
if  the  number  of  .segments  is  n,  checking  the  intersections  of  all  these  segments  can  be 
performed  in  O(n^)  operations.  With  a  distributed  representation  of  the  workspace,  this 
complexity  falls  to  0(n):  as  a  matter  of  fact,  we  can  simply  draw  each  of  the  segments  on 
the  workspace  bitmap.  If  we  draw  a  point  which  has  already  been  drawn,  we  know  that  the 
segments  intersect.  This  remark  shows  once  again  that  distributed  representations  generate 
algorithmic  complexity  measures  which  are  very  different  from  those  obtained  with  algebraic 
representations. 

We  now  detail  the  computation  of  the  configuration  space  potential  field  for  a  generic 
robot. 

Let  us  first  consider  a  simple  bar  in  a  2D  workspace  like  the  one  illustrated  in  figure 
4.2.  We  want  the  bar  to  reach  a  given  goal  configuration  from  any  initial  configuration. 
To  specify  this  goal  configuration  in  terms  of  workspace  potential  fields  applied  to  given 
points  on  the  robot,  selecting  only  one  point  will  not  be  enough  to  characterize  the  goal. 
This  is  so  because  one  point  on  the  robot  specifies  only  two  degrees  of  freedom,  whereas  our 
robot  has  three.  Therefore,  we  need  to  select  at  least  two  points  on  the  robot  if  we  want  to 
specify  completely  the  final  configuration.  For  more  complex  robots  like  manipulator  arms 
with  many  DOF,  one  might  want  to  specify  only  some  points  on  the  end-effector,  in  order 
to  define  a  given  grasping  configuration  without  computing  all  the  inverse  kinematics.  So, 
depending  on  the  application  considered,  the  choice  and  the  number  of  the  attractive  points 
for  defining  the  overall  potential  field  can  be  conveniently  adapted. 

Let  us  call  k  this  number  of  attractive  points,  and  Pi, . . . ,  Pfc  the  corresponding  points  on 
the  robot  body. 

The  overall  configuration  space  potential  field  U{q)  is  computed  as  a  combination  U{q)  = 
G{U'p^{X{pi,q)), . . .  ,Up^{X{pk,q)))  of  several  workspace  potential  fields  f/p,(i), . . . ,  (/p^(x), 
each  of  these  attracting  concurrently  a  given  point  p,  of  the  robot  towards  its  goal  position. 

To  define  precisely  this  overall  potential,  we  still  need  to  specify  the  competition  function 
G.  In  most  of  the  previous  works  on  the  artificial  potential  field  approach,  the  function  G 
was  chosen  linear  [Khatib  86]; 

i  —  k 

Gi7ji,...,yk)  = 

i  =  l 

This  choice  gives  good  results  in  some  cases,  but  the  identification  of  the  multipliers  A,  is  a 
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non-trivial  problem. 

The  choice  of  the  function  G  is  especially  important  because  it  highly  influences  the 
number  of  local  minima  of  the  potential  [/.  With  our  'purfecL'  numerical  potential  fields 
defined  for  a  point  robot,  the  workspace  concavities  do  not  create  any  more  local  minima  by 
themselves.  It  is  the  concurrent  attraction  of  the  different  points  on  the  robot  which  creates 
these  local  minima,  because  of  the  fact  that  these  points  do  not  move  independently.  The 
function  G  precisely  defines  in  which  way  the  competition  between  the  different  points  is 
going  to  be  regulated. 

The  choice  of  G  which  seems  to  minimize  experimentally  the  number  of  local  minima  is 

G(yi,-..,yfc)  =  minyi 

t=l 

This  competition  function  favors  the  attraction  of  the  point  which  is  already  in  the  best 
position  to  reach  its  goal.  It  seems  natural  to  think  that  this  policy  will  minimize  the 
number  of  competitions  between  points,  and  therefore  minimize  the  number  of  local  minima. 
However,  when  one  point  h<LS  reached  its  goal  position,  the  potential  field  is  identically  null, 
and  the  attraction  paradigm  does  not  work  amy  more  to  bring  the  other  points  to  their  goal 
position.  A  solution  to  avoid  this  phenomenon  is  to  add  another  term  to  the  competition 
function; 

{=k 

G(yi,...,yfe)  =  miny,-  +  cmaxy,- 

tsi 

where  e  is  a  small  number.  Then,  we  have  to  tune  the  parameter  e.  In  the  case  of  the  bar  in 
a  2D  workspace  illustrated  on  figure  4.2,  the  best  results  where  obtained  with  e  =  0.1. 

Another  choice  is  to  set; 

i^k 

G'(yi,...,yfc)  =  maxy, 

«=i 

This  choice  would  tend  to  maximize  the  number  of  competitions  between  points,  and  there¬ 
fore  the  number  of  local  minima.  However,  it  can  be  the  best  choice  in  some  cases,  and 
especially  with  manipulator  arms.  As  a  matter  of  fact,  the  number  of  local  minima  is  not 
the  only  measure  for  the  quality  of  the  overall  potential.  Another  very  important  factor  for 
the  convergence  rate  of  the  algorithm  is  the  distribution  of  depths  of  these  local  minima.  The 
method  will  overcome  small  local  minima  without  problem  even  if  they  are  numerous.  But 
only  one  very  deep  local  minimum  can  provoke  the  failure  of  the  whole  algorithm.  Another 
word  for  the  depth  of  a  local  minimum  is  the  activation  energy^  to  refer  to  chemical  kinetics 
terminology.  This  last  choice  of  the  competition  function  seems  to  maximize  the  number  of 
local  minima  but  to  minimize  the  activation  energy  for  each  of  these.  It  is  the  one  we  chose 
for  the  10  DOF  manipulator  arm  illustrated  in  figure  6.5.  In  out  experiments,  we  chose  two 
attracting  points  at  the  two  end-effectors  of  the  non-serial  kinematic  chain. 

It  may  appear  surprising  to  choose  such  competition  functions  involving  computations  of 
maxima  and  minima.  These  are  non-differentiable  for  some  degenerate  points,  and  comput¬ 
ing  their  derivatives  may  cause  numerical  instabilities.  However,  there  are  many  techniques 
for  following  the  gradient  of  a  function,  some  of  which  are  faster  and  fancier,  some  of  which 
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are  slower  and  more  robust.  In  all  cases,  one  can  design  a  differentiable  approximation  of 
the  maximum  function  in  the  following  way:  Choose  a  very  small  number  cr,  and  compute: 

G{yu---,yk)  =  o-log  C^exp{yi/<T) 

\i=l 

One  can  show  that  this  function  uniformly  converges  towards  the  maximum  function  wlien 
cr  converges  towards  zero. 

The  next  section  describes  the  first  of  the  two  paradigm  we  have  used  to  escape  from 
local  minima.  It  is  valid  only  for  a  small  number  of  DOF,  but  happens  to  be  the  fastest  on 
a  sequential  computer  for  robots  with  3  DOF.  A  general  paradigm  valid  for  any  number  of 
DOF  will  be  presented  in  section  6. 

4  Very  Fast  Motion  Planning 
for  a  Small  Number  of  DOF 

4.1  Hierarchical  Best  First  Search  of  Freespace 

We  represent  C  as  a  cartesian  n-dimensional  space  with  modular  arithmetic  for  the  angles. 
For  example,  in  the  case  of  the  3  DOF  mobile  robot  studied  in  section  4.2,  the  configuration 
space  is  represented  by  x  R/27r2.  We  then  perform  a  hierarchical  discretization  of  the 
cartesian  n-dimensional  space  using  a  multiresolution  pyramid.  The  resolution  level  chosed 
in  the  configuration  space  pyramid  is  tightly  related  to  the  resolution  level  chosen  in  the 
workspace  bitmap  representation.  To  make  this  relation  precise,  we  need  to  give  some 
elementary  definitions  related  to  the  concept  of  multiresolution. 

Let  us  denote  6  the  effective  distance  between  two  adjacent  points  in  the  workspace 
bitmap  representation.  In  the  workspace  pyramid,  6  varies  between  6min  and  6maz- 

For  example,  let  us  assume  that  we  have  a  2D  workspace  represented  by  a  pyramid  of 
images  whose  sizes  are  ranging  between  16^  and  512^.  If  the  distance  is  meaisured  in  per¬ 
centage  of  the  workspace  diameter,  we  will  have  in  that  case  =  1/512  and  Sj^ax  =  1/16. 
The  scaling  factor  between  two  successive  resolutions  is  always  2  in  our  implementations, 
although  it  could  be  any  other  factor.  The  resolution  is  by  definition  the  logarithm  in  the 
base  defined  by  the  scaling  factor  of  the  inverse  of  the  distance  between  two  discretiza¬ 
tion  points.  In  our  e.xample,  the  resolution  r  varies  between  r„,„  =  —  log2(i5mar)  =  4  and 

^mar  6. 

For  any  given  resolution  chosen  in  the  workspace  W  C  R"^,  say  r  =  —  log2(<5),  the 
corresponding  resolution  Ri  =  —  log2(  A,)  for  each  of  the  degrees  of  freedom  <7,  must  be  chosen 
in  such  a  way  that  any  elementary  motion  (i.c.  a  motion  of  A,  =  2"^’)  in  configuration  space 
is  transformed  in  a  small  motion  of  all  the  robot  body  points  in  workspace.  By  small  motion, 
we  mean  a  motion  for  which  any  point  on  the  robot  moves  by  less  than  a  few  number  iibtol 
points  in  workspace. 
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The  relation  between  positions  in  workspace  and  configurations  is  given  by  the  forward 
kinematics  relation  X{p,q).  So,  an  elementary  motion  A,  of  q,  in  C  will  generate  a  motion 
of  each  coordinate  Xj  in  workspace  for  each  point  p  on  the  robot  body  of; 


dX^ 

dqi 


If  we  impose  all  workspace  motions  to  be  less  than  nbiol  X  6,  we  must  have: 


A,  =  nbtol  X  6/  sup 

peA,qeCj€[i4] 


=  nbtol  X  6/ 


The  numbers  are  generally  straightforward  to  compute.  Then  the  resolution  Ri  is 
computed  by: 

Ri  =  r  +  log2(J;„p)  -  \og^(nbtol) 

Let  us  consider  as  an  example  a  bar  of  length  L  moving  freely  in  a  2D  workspace.  The  three 
degrees  of  freedom  of  this  bar  are  respectively  the  first  coordinate  xq  of  the  center  of  gravity, 
the  second  coordinate  yc,  and  the  orientation  of  the  bar  9.  xq,  ya,  and  9  are  all  normalized 
coordinates  ranging  between  0  and  1. 

One  can  compute 

j^a  _  JVC  —  1 
'^sup  *'sup  ^ 

and 

«/.up  = 

If  we  set  nbtol  =  2,  we  have; 

Rxc  ^  ^ 

and 

Rg  =  r  +  Iog2(7rL)  -  1 

This  means  that  we  need  2^  =  2  times  less  samples  for  xc  and  yc  than  for  the  workspace 
representation  at  each  resolution,  and  2/{tL)  less  samples  for  9. 

The  uniform  discretization  of  configuration  space  for  each  resolution  level  that  we  just 
described  does  not  take  advantage  of  the  knowledge  we  have  of  the  distance  to  the  obstacles 
Di-  .As  a  matter  of  fact,  instead  of  moving  with  constant  increments  A,  we  could  compute 
for  each  configuration  the  largest  increment  possible,  which  would  allow  the  robot  to  move 
fast,  while  still  avoiding  the  obstacles. 

■As  we  know  our  distance  Di  to  the  obstacles,  we  can  allow  elementary  motions  in 
workspace  of  the  order  of  D\,  instead  of  limiting  ourselves  to  motions  of  the  order  of  6. 
Then,  we  can  approximately  estimate  the  configuration  space  increments  in  the  following 
way: 

A,(D,)  =  nbtol  X  Dx/J'^^p 
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Here,  nbtol  must  be  smaller  than  1  in  order  to  make  sure  that  the  obstacles  are  avoided. 
In  cases  where  the  workspace  is  not  cluttered,  this  adaptation  may  reduce  significantly  the 
overall  computation  time.  However,  it  makes  the  computation  of  the  neighbors  of  a  given 
point  more  complex  than  with  the  uniform  grid. 

The  principle  of  the  approach  at  each  resolution  is  to  perform  a  best  first  search  of  the 
configuration  space  discretized  grid,  using  the  potential  field  as  the  heuristic  for  the  search. 
For  a  n  DOF  robot,  each  discretized  configuration  has  3"  —  1  neighbors. 

The  graph  search  is  first  performed  in  the  coarsest  grid  at  resolution  r^in  in  workspace 
and  Ri  —  Tmin  +  log2('^sup)  ~  log2(n6<o/)  in  configuration  space.  The  workspace  potential 
fields  are  also  computed  at  the  coarse  resolution  r,nin-  If  no  solution  is  found,  the  search 
is  performed  in  the  next  finer  grid,  and  so  on  until  the  finest  grid  is  treated.  At  each 
resolution  level  considered,  the  potential  fields  have  to  be  recomputed.  As  the  computation 
time  required  to  explore  a  coarser  grid  is  almost  neglectable  with  regards  to  the  computation 
time  required  to  explore  a  finer  one,  this  nciive  coarse  to  fine  approach  is  actually  the  best  in 
the  average  case.  It  would  be  possible  to  imagine  a  hierarchical  approach  where  the  resulting 
graph  of  a  given  resolution  could  be  used  as  the  initial  graph  of  the  next  finer  resolution. 
However  the  solution  of  a  path  planning  problem  is  generally  so  versatile  with  respects  to 
the  resolution  that  in  most  cases,  the  resulting  graph  of  the  previous  resolution  where  the 
search  algorithm  has  failed  would  be  more  misleading  than  helpful  for  the  search  in  the  next 
finer  resolution. 

The  search  algorithm  in  each  grid  uses  the  numerical  potential  field  as  the  heuristic  for 
a  best-first  procedure.  As  long  as  the  algorithm  does  not  reach  a  local  minimum  of  the 
potential  field,  the  best-first  search  reduces  to  following  the  gradient  of  the  potential.  When 
a  local  minimum  is  reached,  the  search  algorithm  fills  up  the  attractor  associated  with  this 
local  minimum  until  a  saddle  point  allows  the  best-first  procedure  to  reach  another  attractor, 
and  so  on  until  the  attractor  of  the  goal  is  reached.  At  first  sight,  the  experimental  efficiency 
of  such  a  brute  force  technique  may  appear  fairly  surprising.  The  fundamental  reason  of 
this  efficiency  is  the  following:  The  potential  fields  computed  in  the  workspace  are  designed 
to  be  ‘perfect’  potential  fields  for  a  point  robot,  in  the  sense  that  they  have  no  other  local 
minima  than  the  goal.  Therefore,  complex  shaped  obstacles  with  many  large  concavities  do 
not  create  local  minima  of  the  overall  potential  even  for  a  complex  robot.  Local  minima  can 
occur  only  when  the  workspace  is  so  cluttered  that  the  solution  path  has  to  come  very  close 
to  the  obstacles.  But  in  that  case,  the  number  of  discrete  configurations  lying  in  freespace 
(i.e.  which  do  not  intersect  the  obstacles)  around  the  attractor  associated  with  the  local 
minimum  can  only  be  very  small.  The  attractor  is  therefore  filled  very  fast  by  the  best-first 
search  algorithm. 

This  approach  is  only  valid  for  a  small  number  n  of  DOF,  because  the  number  of  discrete 
configurations  in  any  given  attractor  increases  exponentially  with  the  number  of  DOF,  even 
though  it  is  generally  small  for  n  <  4. 

However,  it  has  two  advantages: 

•  It  is  extremely  fast:  very  tricky  paths  for  a  3  DOF  bar  in  a  2D  workspace  were  obtained 
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Figure  6:  Path  generated  by  a  3  DOF  mobile  robot  in  a  2D  workspace. 

in  about  1  second.  Simpler  paths  without  complex  maneuvers  can  be  obtained  in  about 
1/5  second,  which  can  already  be  considered  real-time. 

•  It  is  deterministically  resolution  complete,  i.e.  the  method  is  guaranteed  to  reach  the 
goal  in  a  finite  amount  of  time  whenever  a  solution  exists  (up  to  the  resolution  of  the 
discretization),  or  return  failure  when  there  is  no  solution. 

The  next  subsection  is  devoted  to  some  experimental  results  obtained  with  this  planner. 

4.2  Experimental  Results 

We  experimented  the  path  planner  on  a  ‘mobile  robot’  with  3  DOF,  namely  a  long  bar  in  a 
2D  workspace. 

The  mobile  robot  is  a  planar  rectangular  object  with  two  DOF  of  translation  and  one 
DOF  of  rotation  (see  figure  6).  Figure  6  shows  an  example  of  path  generated  by  the  planner. 
The  displayed  path  shows  the  ability  of  the  planner  to  produce  complex  maneuvers. 

The  heuristic  potential  was  computed  by  considering  two  points  on  the  robot  body,  at 
the  two  extremities  of  the  bar. 

In  the  setting  shown  in  figure  6,  the  running  time  of  the  algorithm  was  1  second.  This 
compares  extremely  favorably  (three  orders  of  magnitude  faster)  with  the  running  time 
reported  in  [Brooks  83]  for  similar  path  planning  problems.  One  of  these  orders  of  magnitude 
is  obtained  through  the  improvement  of  the  computing  hardware.  The  two  other  ones  are 
actually  a  product  of  our  algorithm. 


Figure  7:  Another  path  generated  among  randomly  distributed  obstacles. 

Figure  7  shows  another  example  of  the  abilities  of  the  algorithm.  Here,  the  same  robot 
found  a  path  in  less  than  5  seconds  within  a  512^  bitmap  world  obstructed  by  more  than 
70  complex-shaped  obstacles.  This  example  demonstrates  a  main  advantage  of  distributed 
representations  over  centralized  ones:  the  running  time  of  the  algorithm  does  not  depend  on 
the  number  and  shape  of  the  obstacles. 

This  hierarchical  best-first  search  algorithm  is  the  fastest  planner  developed  so  far  for 
robots  with  a  small  number  of  DOF.  However,  we  had  to  develop  another  method  based 
on  a  Monte-Carlo  procedure  in  order  to  deal  with  many  DOF.  Of  course,  the  Monte-Carlo- 
based  algorithm  also  works  for  a  small  number  of  DOF.  We  experimented  it  in  the  same 
setting  as  the  one  displayed  in  figure  6.  The  result  obtained  (after  energy  minimization) 
is  shown  in  figure  8.  The  running  time  was  10  second  on  the  same  hardware,  which  is 
ten  times  slower.  However,  as  we  will  see  in  section  7,  the  Monte-Carlo  based  algorithm  is 
massively  parallelizable,  and  will  certainly  prove  extremely  f2ister  than  the  best-first  search 
of  dedicated  computing  hardware.  We  believe  that  real-time  capabilities  are  achievable  with 
this  algorithm. 
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Figure  8:  The  same  path  planning  problem  solved  by  the  Monte-Carlo  procedure. 

5  The  Case  of  Non-Holonomic  Constraints 

5.1  Overview 

This  section  introduces  the  first  algorithm  able  to  deal  systematically  with  maneuvers  for 
the  ‘car  problem’,  i.e.  the  problem  of  planning  the  motion  of  a  car  with  limited  steering 
angle.  This  problem  was  first  introduced  by  [Laumond  86].  Laumond  proved  that  whenever 
a  path  in  open  free  space  does  exist  for  moving  the  body  of  the  car  without  considering  any 
constraint,  then  a  solution  to  the  maneuvering  problem  exists.  However,  the  solution  paths 
which  could  be  obtained  by  implementing  the  constructive  proof  given  in  this  paper  would 
lead  to  a  very  large  number  of  maneuvers,  and  are  therefore  not  practical.  [Laumond  87] 
proposes  an  algorithm  for  planning  the  motion  of  the  car  whenever  there  is  no  need  to 
perform  maneuvers.  The  problem  of  planning  maneuvers  has  been  addressed  for  some  par¬ 
ticular  obstacles  configurations  by  other  authors  [Tourn  88],  but  no  general  solution  has  been 
proposed  yet.  The  main  idea  of  our  approach  is  to  discretize  the  problem  and  perform  a 
dynamic  programming  search  of  the  solution.  The  resulting  algorithm  is  again  resolution 
complete.  Furthermore,  optimality  under  different  criteria  can  be  achieved.  In  particular,  we 
can  constraint  the  algorithm  to  find  the  solution  path  with  the  minimal  number  of  maneuvers 
possible. 

The  next  subsection  is  devoted  to  a  characterization  of  holonomy  for  mechanical  systems 
using  Frobenius  Integrability  Theorem.  This  characterization  allows  us  to  prove  the  non- 


holonomicity  of  the  ‘car  problem’.  The  reader  familiar  with  holonomy  may  want  to  skip  this 
subsection. 

5.2  A  Local  Characterization  of  Linear  Holonomy. 

We  have  considered  so  far  the  motion  planning  problem  as  a  pure  geometric  problem,  i.e. 
a  problem  for  which  all  the  constraints  can  be  expressed  using  only  the  geometric  variables 
(<7i,  •  • . ,  9n).  In  other  words,  a  constraint  is  said  to  be  geometric  when  is  can  be  expressed  as 
an  equation  in  configuration  space: 


Fiq,t)  =  Q,  qeC  (1) 

Such  a  constraint  can  be  interpreted  geometrically  in  the  following  way  (if  the  function 
F  is  smooth  and  has  a  surjective  differential);  equation  1  defines  a  submanifold  of  the 
configuration  space  C,  and  it  means  that  the  mechanical  system  considered  is  bound  to  stay 
on  this  submanifold  whatever  force  is  applied  to  it. 

A  real  world  mechanical  system  can  have  many  kinds  of  constraints,  some  of  which  can  be 
reduced  to  a  geometrical  relation,  some  others  which  cannot.  In  particular,  one  can  consider 
the  larger  class  of  kinematic  constraints,  i.e.  the  constraiints  which  include  the  geometric 
parameters  and/or  velocity  parameters.  MathematicaJly,  any  kinematic  constraint  can  be 
expressed  as: 

G(«,?,1)  =  0.  (9.9)€TB(C)  (2) 

where  TB{C)  is  the  tangent  bundle  associated  with  the  manifold  C,  i.e.  the  space  representing 
the  configurations  and  velocities.  The  tangent  bundle  is  itself  a  manifold  [Spivak  79].  It  is 
more  commonly  called  phase  space  in  Physics,  or  state  space  in  control  terminology. 

When  dealing  with  a  general  kinematic  constraint  of  the  form  2,  am  interesting  problem 
is  to  know  if  the  constraint  can  be  integrated,  i.e.  if  the  system  of  first  order  differential 
equations  defined  by  2  can  be  reduced  to  a  set  of  geometric  relations  of  the  form  1.  This 
problem  is  commonly  referred  to  in  mechanics  as  the  holonomy  problem. 

A  constraint  of  the  form  2  will  be  said  to  be  holonomic  if  it  can  be  integrated,  and 
non-holonomic  otherwise.  To  our  knowledge,  there  is  no  mathematical  result  about  the 
integrability  of  equations  of  the  form  2  in  the  general  case.  However,  there  is  a  very  important 
particular  case  for  which  a  characterization  can  be  derived.  We  detail  this  particular  caise  in 
the  next  few  paragraphs. 

Kinematic  constraints  on  mechanical  systems  are  generally  the  result  of  a  rolling  contact 
between  two  rigid  bodies.  For  example,  in  the  case  of  the  ‘car  problem’,  the  kinematic 
constraint  is  the  result  of  the  rolling  of  the  wheels  on  the  ground.  The  kinematic  constraint 
is  expressed  as  a  relation  between  the  relative  velocities  of  the  two  points  in  contact.  In 
general,  the  contact  is  a  combination  of  rolling  and  sliding,  this  combination  depending  on 
the  friction  coefficients  of  the  two  bodies.  When  there  is  no  sliding  at  all,  the  relation  simply 
states  that  the  velocities  of  the  two  points  in  contact  are  the  same,  relation  which  is  linear 
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in  the  velocity  parameters.  Therefore,  the  case  where  the  function  G  of  formula  2  is  linear 
as  a  function  of  the  velocities  is  particularly  interesting.  Under  the  assumption  of  perfect 
rolling,  equation  2  can  be  rewritten: 

i=n 

G{q,q,t)  =u{q,t).q  =  ^uJi{q,t)qi  =  0  (3) 

«=i 

For  each  configuration  q,  u){q,t)  is  linear.  By  definition,  u)  is  called  a  I -differential  form 
([Spivak  79]).  For  each  q  equation  3  defines  an  hyperplane  iS.{q)  included  in  the  tangent 
space  of  C  at  q.  The  function  A  is  called  (n-t)-distribution  associated  to  u;. 

There  is  a  fundamental  result  about  integrability  of  linear  kinematic  constraints  called 
Frobenius  Integrability  Theorem^  which  can  be  simply  stated  as  follows  when  applied  to 
1- forms: 


Theorem  1  (Frobenius  Integrability  Theorem) 

Let  (jj  be  a  l-/orm  on  a  manifold  C  of  dimension  n,  and  A  the  associated  distribution  of 
hyperplanes. 

In  a  neighborhood  of  any  point  qo  €  C,  the  three  following  conditions  are  equivalent: 

i)  u;  A  dw  =  0  (i.e.  the  exterior  product  between  u)  and  its  exterior  differential  is  null). 

ii)  The  distribution  A  is  invariant  under  the  Lie  bracket,  [i.e.  for  any  couple  of  vector  fields 
{X,Y)  on  A,  [X,Y]  is  also  on  A). 

iii)  There  is  a  foliation  of  C  tangent  to  A.  (i.e.  the  constraint  3  can  be  integrated) 


A  proof  of  this  theorem  in  the  general  case  can  be  found  in  [Spivak  79).  In  the  case  of 
1- forms  stated  above,  a  pedestrian  proof  only  based  on  elementary  calculus  (Fixed  point 
theorem)  can  be  found  in  [Barra  88).  It  is  important  to  notice  that  this  local  result  can  be 
globalized  ([Spivak  79],  vol.  1,  pp  264-268).  From  this  theorem,  we  are  able  to  infer  a  local 
characterization  of  holonomy  for  linear  kinematic  constraints,  and  in  particular  of  any  kind 
of  kinematic  constraint  induced  by  pure  rolling  contacts. 

By  definition  of  exterior  differentiation  of  differential  forms,  we  have: 


du;=  ^ 
1 


dq,  A  dqj 


From  the  definition  of  exterior  product  of  differential  forms  and  the  above  formula  we  com¬ 
pute: 


dqk) 


dq,  A  dqj  A  dqi, 


Therefore,  we  deduce  from  Frobenius  Theorem: 
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Figure  9:  The  Car  Problem. 


Corollary  1  (Characterization  of  linear  holonomy) 

A  linear  kinematic  constraint  (and  in  particular  a  pure  rolling  constraint)  defined  by: 

issn 

G{q,q,t)  =  (^iq,t}.q  =  Y^UJi{q,t)qi  =  0 

•=i 

IS  holonomic  if  and  only  if  the  following  relation  holds  for  any  {i,j,k)  6  [l»^P  such  that 
i<i<j<k<n: 


duk  dijjj 


dui  dujk 


^  Ur  - 


dqj  dqk 


dqk  dqi 


+  '^k 


'  duj  du>i ' 

.  ^<7.  dqj , 


=  0 


In  the  next  subsection,  we  state  the  ‘car  problem’,  and  prove  its  non-holonomicity  using 
the  above  result.  This  property  implies  that  the  ‘car  problem’  cannot  be  handled  by  purely 


geometric  algorithms. 


5.3  The  Car  Problem 

Let  us  consider  a  front  wheel  drive  car  with  limited  steering  angle.  Figure  9  displays  a 
simple  model  of  such  a  car  viewed  from  the  top.  Such  a  mechanical  system  hais  3  DOF 
(two  translations  and  one  orientation),  but  it  cannot  change  the  three  configuration  space 
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parameters  independently.  This  is  so  because  the  velocity  of  the  car  rear  is  always  tangent 
to  the  car  orientation:  the  car  has  a  kinematic  constraint^. 

The  car  configuration  is  parameterized  by  the  coordinates  Xj  and  Yj  of  the  driver  (the 
point  F  in  the  middle  of  the  two  front  wheels),  and  the  car  orientation  0.  The  steering  angle 
of  the  front  wheels  is  denoted  by  6  [~4>maxi  F4>inax\-  The  car  can  move  forward  y  >  0  or 
backward  u  <  0.  The  length  of  the  car  is  L. 

The  coordinates  (A^jK)  of  the  point  R  in  the  middle  of  the  rear  wheels  verify  the 
following  relation: 

X,=Xj-Lcos6  Y,  =  Yj-Ls\nO  (4) 

The  kinematic  constraint  expresses  that  the  velocity  of  R  is  parallel  to  the  orientation  of  the 
car: 

Xr  =  A  cos  9  Yr  =  \  sin  9 
where  A  is  a  constant.  Eliminating  A,  we  get: 

—  ArSinS  +  Kcos0  =  0  (5) 

Taking  the  derivative  of  formula  (4)  we  get: 

X,=Xj  +  9Lsm0  Yr  =  Yj  -9Lcos9 
Combining  (5)  and  (6)  we  obtain  the  kinematic  constraint: 

—  XjsiaO  +  Yj  cos  0  —  0L  =  0 
Using  the  notations  of  the  previous  subsection,  we  identify: 

ujXj  =  —  sin0  u}Yf  =  cost?  uJe  =  ~L 
We  compute  the  coefficient  AxjYfe'- 


(6) 

(’7) 


[dYf  d9  1  ^>\d9  dXj)  \dXj  dYj  ) 


1  ^  0 


Therefore,  the  kinematic  constraint  of  the  car  problem  is  non-holonomic. 

W'e  now  derive  the  equations  governing  the  control  of  the  system.  As  the  car  is  a  front 
wheel  drive,  the  velocity  of  F  is  computed  as: 


X f  =  V  cos(0  +  <f>)  Yf  =  V  s\n{0  +  (8) 

'Here  we  model  this  constraint  using  only  the  3  DOF  necessary  to  plan  the  motion  of  the  car.  In  fact, 
a  real  car  has  many  more  DOF  and  many  kinematic  constraints.  In  particular,  the  constraint  that  we  are 
simply  modeling  here  is  induced  by  the  pure  rolling  contacts  between  the  wheels  and  the  ground.  We  do  not 
need  to  go  into  such  a  detailed  modelization  for  our  purpose. 
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Combining  this  with  the  kinematic  constraint  (7)  we  get: 

vs\a<i>  —  LO  =  0 

Finally,  we  obtain  the  full  system  of  differential  equations  relating  the  control  parameters 
{v,4>)  to  the  configuration  parameters  {Xf,Y/,6)  by  combining  formula  (8)  and: 

•  sin<^ 

This  system  of  differential  equations  is  non-linear,  and  has  to  be  integrated  numerically 
when  the  controls  are  time-varying.  However,  whenever  v  and  ^  remain  constant,  it  can  be 
solved  analytically.  There  is  nothing  surprising  with  that;  with  a  given  constant  steering 
angle  and  at  a  given  speed,  the  trajectory  of  the  point  F  is  an  arc  of  circle,  while  the 
orientation  of  the  car  changes  linearly.  The  integration  is  straightforward.  We  first  compute 

«(()  =  «(0)+l^^  (9) 

and  then  Xf  and  Y/  using  the  above  result: 

Xj{t)  =  Xf{Q)  -h  (s\n{<l>  +  0(0)  +  -  sin(<^  -h  0(0))^  (10) 

sin  4>  \  L  J 

Y,(l)  =  n(l))  -  ^  +  m  +  -  cos(^  +  9(0))')  (11) 

sm  <p  \  L  / 

From  the  two  last  formulas,  we  deduce  that  the  turning  radius  of  the  car  is 


The  algorithm  we  use  to  find  a  path  between  two  given  configurations  is  very  similar  to 
the  one  used  in  the  holonomic  case.  The  main  difference  comes  from  the  computation  of  the 
neighbors  of  a  given  configuration.  In  the  holonomic  case,  control  space  and  configuration 
space  are  identical.  This  is  not  any  more  true  in  the  non-holonomic  case.  The  idea  here  is  to 
discretize  the  control  parameters  {v,  4>)  instead  of  discretizing  the  configuration  parameters. 
For  example,  we  can  set  the  number  of  neighbors  to  6,  corresponding  to  the  6  following 
control  values; 

V  €  {-Uo.Uo},  ^6  {-<i>max,0,+d>max] 

The  configurations  of  the  6  neighbors  are  then  computed  from  formulas  (9),  (10),  (11). 

We  could  perform  the  search  in  a  best-first  faishion  like  we  did  in  the  holonomic  case. 
This  would  result  in  admissible  paths,  but  these  solutions  would  include  a  very  large  number 
of  maneuvers.  The  objective  function  used  in  the  dynamic  programming  search  is  in  general 
a  combination  of  the  heuristic  potential  field  term  and  the  term  relative  to  the  number  of 
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Figure  10:  A  Parking  Maneuver. 


maneuvers.  If  we  want  to  minimize  the  number  of  maneuvers,  we  have  to  modify  the  best- 
first  procedure.  The  idea  is  to  use  the  precomputed  distance  to  the  obstacles  Di,  and  to 
compare  it  to  the  turning  diameter  Dj  =  2Rr  =  21/  s\n4>max-  As  long  zis  the  robot  is  far 
from  the  obstacles  and  the  goal  {Di  >  Dx),  we  can  proceed  in  a  best-first  fashion  with 
no  risk  to  need  maneuvers.  When  the  robot  comes  too  close  to  the  obstacles  or  the  goal 
(Di  <  Dj),  we  have  to  abandon  the  potential  field  heuristic  and  to  perform  the  search  using 
only  the  term  relative  to  the  number  of  maneuvers. 

Examples  of  paths  for  the  car  problem  with  complex  maneuvers  are  shown  and  discussed 
in  the  next  subsection. 

5.4  Experimental  Results 

We  experimented  the  non-holonomic  car  planner  with  various  values  of  the  maximal  steering 
angle  Figure  10  shows  an  example  of  the  parking  problem  with  a  very  limited  steering 

angle  Pmor  =  30  degrees.  The  running  time  was  30  seconds.  This  is  much  longer  than  the 
running  time  in  the  holonomic  case.  The  reason  is  that  with  such  a  limited  steering  angle, 
the  turning  diameter  is  larger  than  the  workspace  diameter  itself.  Therefore,  the  heuristic 
potential  field  does  not  help  much  in  that  case  to  obtain  the  final  solution.  The  algorithm 
has  to  explore  a  large  part  of  the  search  space. 

Figure  11  shows  an  example  of  maneuvering  in  a  clustered  environment  with  a  steering 
angle  o^az  —  'IS  degrees.  The  running  time  was  about  1  minute.  Figure  12  displays  four 


Figure  11:  Maneuvering  in  a  clustered  environment. 


successive  parts  of  the  same  path  so  that  the  different  maneuvers  can  be  isolated  from  each 
other.  Ten  maneuvers  (i.e.  changes  on  the  sign  of  v)  were  necessary  in  this  example. 

Any  kind  of  maneuver  can  be  performed  in  any  obstacle  distribution,  up  to  the  resolution 
of  the  bitmap. 

6  Large  Systems:  A  Monte-Carlo  Approach 

6.1  Overview 

The  hierarchical  best-first  search  algorithm  described  in  the  previous  sections  camnot  plan 
the  motion  of  robots  with  many  DOF.  The  reason  of  this  limitation  is  the  same  for  any 
complete  motion  planning  algorithm  based  on  the  configuration  space  approach:  the  volume 
of  configuration  space,  which  is  locally  diffeomorphic  to  R",  increases  exponentially  with 
77.  One  the  other  hand,  human  beings  are  able  to  solve  motion  planning  problems  with 
a  very  high  number  of  DOF.  Often,  the  redundancy  of  DOF  is  helpful  and  simplifies  the 
planning  problem.  There  is  a  divorce  between  the  mathematical  result  of  NP-hardness  and 
the  everyday  life  experience.  To  design  realistic  efficient  motion  planners  for  many  DOF,  we 
/inre  lo  drop  the  completeness  requirement.  Then,  the  main  question  is  the  following:  Is  it 
possible  to  design  non-complete  algorithms  which  give  nevertheless  the  correct  solution  in 
most  real-life  cases? 

The  main  purpose  of  this  section  is  to  show  some  experimental  results  which  let  us  think 
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Figure  12:  Maneuvering  in  a  clustered  environment  (continued). 
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that  the  answer  is  positive. 


6.2  Local  Minima:  The  Exit  Problem 

The  Monte-Carlo  approach  that  we  describe  in  this  subsection  uses  the  same  framework  as 
the  graph  search  approach.  It  uses  the  same  numerical  potential  field  U,  and  is  implemented 
with  the  same  coarse-to-fine  hierarchical  decomposition  of  the  world  model.  As  there  is 
no  interaction  between  the  search  at  different  levels  of  hierarchy,  all  these  levels  could  be 
implemented  concurrently  on  a  parallel  machine.  The  difference  comes  from  the  way  local 
minima  of  the  potential  field  are  treated.  We  detail  here  the  procedure  employed  at  a  given 
resolution  level.  The  principle  is  the  following. 

Starting  from  the  initial  configuration  qiNiTy  we  first  follow  the  gradient  of  the  numerical 
potential  field  U{q)  until  we  reach  a  local  minimum  qioc  at  altitude  Uioc  =  ^(qioc)-  The 
problem  of  exiting  from  this  local  minimum  is  solved  by  generating  several  random  motions 
from  qioc,  each  of  these  random  motions  having  a  random  duration.  The  two  key  points  of 
the  algorithm  are  how  to  perform  these  random  motions,  and  how  to  choose  their  duration. 
Before  detailing  these  two  points  in  the  next  subsection,  we  give  the  overadl  sketch  of  the 
algorithm.  From  all  the  termination  configurations  of  these  random  motions,  we  follow 
again  the  gradient  of  the  potential  field  until  we  reach  new  local  minima.  We  retain  only 
those  of  the  new  local  minima  whose  altitude  is  lower  than  Uioc-  If  none  of  the  new  local 
minima  has  an  altitude  lower  than  Uioc,  we  say  that  qioc  is  a  dead-end.  Otherwise,  for  each 
of  those  new  local  minima  retained,  we  perform  again  the  random  motion  procedure,  in  a 
best-first  fashion.  So,  a  graph  of  local  minima  is  incrementally  built,  the  path  joining  two 
neighbor  local  minima  being  the  concatenation  of  a  random  motion  and  a  gradient  motion. 
A  standard  best-first  search  of  this  graph  is  performed  until  we  reach  the  goal,  or  until  all 
the  local  minima  developed  are  dead-ends. 

When  the  solution  path  is  finally  obtained,  we  smooth  it  using  the  classical  principles  of 
Variational  Calculus.  More  precisely,  we  minimize  the  total  work  needed  to  move  the  robot 
from  the  initial  configuration  to  the  goal  configuration. 

.4  very  nice  property  of  this  procedure  is  that  all  the  brownian  motions  starting  from  a 
given  local  minimum  can  be  performed  in  parallel  on  any  SIMD  machine  (there  is  no  need 
for  intercommunication  between  the  different  processing  units). 

.As  we  use  a  random  procedure  to  build  the  graph  of  local  minima,  we  can  never  be  sure 
to  find  a  path  whenever  it  exists.  In  other  words,  the  technique  is  not  deterministically 
resolution-complete.  However,  the  fundamental  properties  of  Brownian  motion  allow  to 
prove  that  when  the  computation  time  converges  towards  infinity,  the  probability  to  reach 
the  goal  converges  towards  1.  We  will  say  that  our  Monte-Carlo  procedure  is  probabilistically 
resolution-complete.  This  convergence-in-distribution  property,  well-known  for  the  so-called 
‘simulated  annealing’  algorithms  [Geman  86],  is  a  very  weak  one.  To  illustrate  its  weakness, 
let  us  consider  the  most  silly  (uninformed)  of  the  motion  planning  algorithms  that  one 
can  imagine;  Start  a  Brownian  motion  from  the  initial  configuration  without  using  any 
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heuristic,  and  keep  on  until  reaching  a  small  neighborhood  of  the  goal  configuration.  This 
entirely  uninformed  algorithm  is  probabilistically  resolution-complete!  What  the  theory  does 
not  tell  is  how  long  time  the  algorithm  will  need  to  actually  reach  the  goal  (and  it  should 
better  not  tell!).  Designing  algorithms  only  makes  sense  when  one  has  some  control  of  the 
computational  complexity  in  the  average  case. 

The  potential-field-informed  Monte-Carlo  procedure  that  we  propose  happens  to  be  ex¬ 
perimentally  efficient  for  motion  planning.  Again,  the  efficiency  of  such  a  simple  technique 
may  appear  fairly  surprising.  We  believe  that  the  reason  of  its  efficiency  is  the  following; 
The  mathematical  problem  of  motion  planning  when  stated  in  algebraic  terms  is  known 
to  be  Pspace-hard  in  the  number  n  of  DOF.  On  the  other  hajid,  the  more  DOF  a  robot 
manipulator  has,  the  more  paths  there  are  to  reach  a  given  configuration  from  any  initial 
configuration.  The  redundancy  of  DOF  often  appears  helpful  when  human  beings  are  con¬ 
fronted  to  a  given  planning  problem.  In  other  terms,  the  more  DOF  you  have,  the  more 
solutions  to  the  planning  problem  you  have.  The  worst-case  behaviour  of  the  problem  (very 
large  search  space  with  very  few  solutions)  does  not  appear  often  in  real-life  cases.  The 
average  case  behaviour  seems  to  be  more  of  the  type:  very  large  search  space  with  very  large 
number  of  solutions.  In  such  cases,  controlled  stochastic  algorithms  can  be  particularly  well 
suited. 

Monte-Carlo  procedures  have  already  been  helpful  in  some  cases  for  solving  NP-hard 
problems.  [Cerny  85]  gives  sub-optimal  solutions  to  the  traveling  salesman  problem  for  more 
than  10000  towns.  (Kirk  83]  proposes  an  algorithm  better  than  human  experts  for  placement 
and  routing  of  VLSI  chips.  The  traveling  salesman  problem  is  indeed  NP-hard,  but  the  very 
large  search  space  is  associated  with  a  very  large  number  of  ‘good’  sub-optimal  solutions.  It 
is  the  same  for  routing  of  VLSI. 

Monte-Carlo  procedures  for  optimization  have  also  been  used  more  or  less  successfully 
in  many  other  fields  of  science  and  engineering,  and  in  particular  in  Image  Processing. 
[Geman  84]  applies  a  so-called  ‘simulated  annealing’  approach  for  restoring  images  blurred 
with  non-linear  filters.  Several  authors  have  implemented  edge  detection  algorithms  based 
on  the  same  paradigm.  Recently,  it  has  been  applied  to  higher  level  problems  in  Computer 
Vision.  [Barnard  88]  addresses  the  stereo  matching  problem  using  a  hierarchical  pixel-level 
simulated  annealing  algorithm.  [Barra  88]  proposes  a  new  approach  to  the  3D  reconstruc¬ 
tion  of  stratigraphic  layers  and  the  detection  of  geological  faults  in  seismic  data.  Despite 
some  impressive  results  for  specific  problems,  the  Monte-Carlo  approach  to  Computer  Vision 
cannot  be  considered  as  a  general  paradigm.  There  is  a  straightforward  reason  to  this  lack 
of  popularity:  the  search  space  of  Computer  Vision  problems  is  huge.  Furthermore,  these 
problems  are  hard  to  express  correctly  in  terms  of  the  optimization  of  a  functional.  When 
it  is  possible,  several  parameters  have  to  be  tuned  empirically  or  by  means  of  identification 
models,  leading  to  even  heavier  optimization  problems. 

For  motion  planning,  the  number  of  parameters  has  been  reduced  to  a  tractable  set 
using  the  specific  characteristics  of  the  problem,  and  the  function  to  optimize  is  well  defined. 
Furthermore,  the  Monte-Carlo  procedure  proposed  behaves  very  differently  from  the  classical 
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‘simulated  annealing’  procedures.  Basically,  simulated  annealing  procedures  perform  a  kind 
of  breadth-first  search  of  the  graph  of  local  minima  of  the  function  to  be  optimized,  whereas 
our  Monte-Carlo  algorithm  performs  a  best-first  search  of  this  graph  using  as  heuristic  the 
function  to  optimize  itself.  We  believe  that  this  best-first  approach  leads  to  much  higher 
convergence  rates.  This  belief  is  supported  by  strong  experimental  evidence  in  the  case  of 
Robot  Motion  Planning. 


6.3  What  Is  a  Continous  Random  Motion? 

We  now  detail  the  first  key  point  of  the  algorithm,  that  is  how  to  perform  a  random  motion. 
When  we  reach  a  local  minimum  of  the  potential  field  U,  there  is  no  more  information  that 
we  can  extract  locally  from  U  to  guess  the  direction  of  motion  which  will  lead  to  the  goal.  In 
some  way,  the  potential  gave  us  all  what  it  knows  locally  about  the  goal.  /4s  long  as  we  do  not 
make  any  assumption  on  the  statistics  of  the  obstacles  distribution,  we  have  no  more  piece  of 
valuable  information  to  reach  the  goal.  Then,  if  we  do  not  want  our  algorithm  to  stop  and 
return  failure,  we  have  to  choose  the  next  direction  without  any  information.  As  we  work  at 
a  given  resolution  R,  =  —  logj  Aj  for  each  configuration  space  parameter  9,-,  we  would  like 
the  standard  deviation  of  our  step  to  be  equal  to  A,-.  But  the  most  uninformed  distribution 
(i.e.  the  one  which  maximizes  the  entropy)  among  all  those  which  have  a  given  standard 
deviation  A,-  is  precisely  the  Gaussian  distribution  [Roubine  70].  Therefore,  our  velocity 
along  each  configuration  parameter  must  follow  a  Gaussian  distribution  with  deviation 
A,.  But  the  problem  of  choosing  our  velocity  happens  again  and  again  at  every  single 
time,  independently  of  the  previous  times.  Therefore,  the  velocity  process  n(t)  must  be  a 
time-decorrelated  Gaussian  process.  By  time-decorrelated  we  mean  that  the  autocorrelation 
function  is; 

r„(t,0  =  Af^(t-0 

where  6  is  the  Dirac  function,  or  equivalently^  that  the  power  spectrum  is  flat: 

Sn(0  = 


This  is  precisely  the  definition  of  a  white  Gaussian  noise  .  It  can  be  proven  that  the  white 
Gaussian  noise  is  the  generalized  derivative"*  of  the  standard  Brownian  motion,  also  called 
Wiener-Levy  process.  Therefore,  the  most  uninformed  motion  (5(0  =  ((5i(0)  •  ■  •  1  *3"(0) 
configuration  space  is  the  Brownian  motion.  The  Wiener- Levy  process  is  a  non-stationary  ho¬ 
mogeneous  centered  Gaussian  Markovian  process  with  the  following  properties  [Papoulis  65]: 


Its  density  is  given  by: 


^This  equivalence  results  from  the  Bochner-Wiener-Khintchine  theorem  [Feller  66]:  The  spectral  power 
is  the  Fourier  transform  of  the  autocorrelation  function  for  any  stationary  process. 

■^derivative  in  the  sense  of  stochastic  distributions  (Roubine  70]  [Fernique  67]. 
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•  The  standard  deviation  of  increments  E(t)  increases  proportionally  to  the  square  root 
of  the  duration: 

S^(t)  =  E  +  t)-  =  AU 

•  The  increments  are  independent,  i.e.  the  two  processes  (5(0  —  <5(^0  Q{^')  ~  Qi^") 

are  independent  for  any  (t,  t\  t") 

It  can  be  shown  that  the  Wiener- Levy  process  is  almost  surely  continous^,  but  this  is 
a  highly  non-trivial  result!  The  Wiener-Levy  process  is  well-defined  as  long  as  it  does  not 
encounter  any  obstacles  in  configuration  space.  When  the  process  Q{t}  hits  the  bound¬ 
ary  of  a  C-obstacle,  the  Brownian  motion  procedure  has  to  be  adapted  in  order  to  remain 
in  freespace.  The  classicaJ  generalization  of  Brownian  motion  when  the  space  is  bounded 
is  called  reflective  boundary  motion.  Basically,  it  consists  of  reflecting  symmetrically  to  the 
tangent  hyperplane  of  the  C-obstacle  the  motion  that  would  take  place  if  there  were  no  obsta¬ 
cle.  The  mathematical  consistency  of  this  reflective  boundary  assumption  is  fully  discussed 
in  [Anderson  76].  We  will  not  detail  it  in  this  paper. 

We  now  detail  the  second  key  point  of  the  algorithm,  that  is  how  to  choose  the  duration 
of  the  Brownian  motions  Q{t).  Basically,  we  want  these  motions  to  talce  the  robot  out  of  the 
local  minima  of  U.  For  each  local  minimum  qioc  of  C/,  we  cam  consider  its  scope,  or  attraction 
radius  A/j,(9/oc)  in  each  direction  qi.  This  scope  is  the  distance,  along  each  configuration 
space  parameter  qi,  between  g/oc  ^.nd  the  neaucest  saddle  point  qsadcUe-  To  escape  from  a  local 
minimum,  we  simply  have  to  exit  the  attractor  of  qioc-  To  do  so,  the  minimum  distance 
that  we  have  to  travel  in  each  direction  qi  from  qiod  is  precisely  Aiii{qioc)-  If  we  are  able  to 
estimate  the  statistics  of  Ar,  the  property  S(t)  =  A,  v^  gives  us  a  clue  for  the  exit  problem. 
The  duration  T{qioc)  of  fhe  Brownian  motions  should  be  of  the  order  of 


T(?/oc)  «  max 


(12) 


But  to  estimate  the  statistics  of  Ar,  we  need  to  have  a  statistical  model  of  the  potential 
field  U.  As  we  do  not  make  any  assumption  on  the  obstacles  distribution,  we  cannot  infer 
any  statistical  property  about  U  and  Ar.  Generally,  we  may  assume  that  the  distance 
for  each  parameter  qi  will  not  exceed  the  value  that  would  provoke  a  motion  in  workspace 
of  more  than  the  workspace  diameter  itself.  This  diameter  being  1  with  our  normalized 
coordinates,  we  obtain  the  following  estimate  of  Ar,  for  any  local  minimum  qioc'- 


ARi(qioc)  «  I/<4up 

On  the  other  hand,  we  have  A,  %  where  <5  is  the  distance  between  two  points  in 

workspace.  Combining  these  two  formulas  with  (12),  we  obtain: 

nq,^)  =»  ^ 

*in  other  words,  the  probability  for  being  discontinuous  at  any  given  point  is  zero 
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VVe  now-  have  to  choose  for  T  a  single- parameter  family  of  distributions  to  define  completely 
our  algorithm.  All  what  we  know  about  the  attraction  radii  Ar,  is  that  they  are  strictly 
positive,  and  that  their  expected  value  is  bounded  by  l/Jj'up-  Again,  the  most  uninformed 
distribution  (i.e.  the  one  which  maximizes  the  entropy)  of  a  real  positive  process  whose 
expected  value  is  given  is  the  truncated  Laplace  distribution.  The  density  of  A/?,  is  therefore 
given  by: 

p(An,)  =  J‘„pexp(-J,’„pARi) 

The  intuitive  interpretation  of  this  result  is  the  following:  as  no  hypothesis  is  made  about 
the  shape  of  the  potential  field  f/,  the  maximum  entropy  paradigm  states  that  the  rate  of 
variation  of  the  number  N  of  attractors  having  a  given  radius  An  is  constant.  In  other 
words,  IfN  X  dN/dAn  does  not  depend  on  Ar.  This  means  that  the  overall  statistical  shape 
of  the  potential  does  not  change  when  the  resolution  varies.  But  1/iV  x  dNfdAn  equals 
1/p  X  dpIdAn.  Therefore  p  varies  exponentially  with  Ar,  which  precisely  defines  a  Laplace 
distribution. 

Consequently,  the  relation  (12)  induces  the  following  distribution  for  the  duration  T: 

P{i)  =  ■^€:xp{-SVt)  (13) 

One  can  verify  that  the  expected  value  of  this  distribution  is  indeed  1/P. 

The  Monte-Carlo  procedure  is  now  completely  defined.  However,  the  final  path  obtain  is 
the  concatenation  of  several  gradient  motions  and  Brownian  motions.  It  does  not  have  good 
dynamic  properties.  Therefore,  we  postprocess  this  path  by  means  of  an  energy  minimization 
procedure  that  we  describe  now. 

6.4  Path  Optimization 

There  are  several  Riemanian  metrics  that  we  can  consider  on  the  configuration  space  manifold 
C.  The  most  natural  one  is  the  kinetic  energy  metric,  but  it  becomes  a  complex  expression 
for  large  systems.  In  our  implementation,  we  chose  a  very  simple  metric,  such  that  the 
geodesics  of  the  metric  are  easy  to  compute.  For  some  simple  unit  mass  mobile  robots  (like 
the  long  bar  studied  in  section  4.2),  this  metric  is  exactly  the  kinetic  energy  metric.  For  long 
manipulator  arms  including  several  revolute  joints  in  serie  (like  the  10  DOF  robot  shown  in 
section  6.5),  it  is  no  longer  the  case.  We  used  the  metric: 

ds^  =  dq] 
i-\ 

where  the  (y,  are  normalized  coordinates  (i.e.  all  ranging  between  0  and  1).  The  geodesics 
equation  reduces  in  this  case  to: 

7(0  =  0 
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with 


libNir)  =  <lfNlT->  li^GOAc)  =  Qgoal 

This  is  a  classical  quadratic  two  points  boundary  value  problem  under  the  non-linear  obstacle 
avoidance  constraint.  It  can  be  solved  using  standard  projected  gradient  procedures  such 
as  Gauss-seidel  with  projection  (on  a  sequential  computer)  or  Jacobi  with  projection  (on  a 
parallel  machine).  The  final  path  obtained  is  smooth,  and  of  course  homotopic®  to  the  initial 
path  obtained  from  the  Monte-Carlo  procedure. 

At  this  point,  it  is  necessary  to  make  some  remarks  about  the  topological  properties  of 
the  configuration  space,  depending  on  the  dimension  of  the  workspace.  If  the  robot  is  a  solid 
object  moving  in  a  2D  workspace  (like  the  long  bar),  its  configuration  space  is  x  5'*; 
for  translations,  5‘  for  rotations.  The  Poincare  group^  of  this  space  is: 

n(R*  X  5‘)  =  Z 

This  simply  means  that  the  robot  can  maJee  any  number  of  turns,  either  clockwise  or  counter¬ 
clockwise.  Any  couple  of  paths  having  the  same  total  number  of  turns  are  homotopic,  but  two 
paths  with  different  numbers  of  turns  can  never  be  homotopic.  This  result  has  an  important 
consequence  for  our  energy  minimization  procedure:  if  the  Monte-Carlo  algorithm  outputs  a 
path  having  more  turns  than  necessary,  the  smoothing  procedure  with  not  solve  the  problem. 
In  fact,  some  cosmetic  improvements  can  be  made  to  the  Brownian  motion  generator  in  order 
to  avoid  this  problem.  We  will  not  detail  it  is  this  paper. 

Now,  if  the  robot  is  a  solid  object  in  3D  (free-flying  robot),  its  configuration  space  is 
R^  X  S0(3):  R^  for  translations,  S0(3)  (special  orthogonal  group)  for  rotations.  The 
Poincare  group  of  this  space  is 

n(R^  X  50(3))=  {0,1} 

This  means  that  there  are  only  two  different  homotopy  classes:  the  class  of  paths  which 
make  a  total  rotation  of  27r,  and  the  class  of  paths  which  make  a  total  rotation  of  0.  Any 
path  can  be  shown  to  belong  to  one  of  these  two  kinds  by  changing  continuously  the  axis 
of  rotation.  Therefore,  even  if  the  Monte-Carlo  procedure  outputs  a  path  with  more  turns 
than  necessary,  the  energy  minimization  procedure  will  reduce  the  number  of  turns  to  the 
optimal  possible. 

Finally,  if  the  robot  is  a  fixed-base  manipulator  arm  with  mechanical  stops  on  the  revolute 
joints,  the  configuration  space  is  simply  connected,  and  equivalent  to  an  n-cube  in  R".  Its 
Poincare  group  is  extremely  simple; 

mR”)  =  (0) 

Tlie  homotopy  problem  does  not  exist  for  such  manipulators. 

'’i.e.  the  final  path  is  obtained  from  the  initial  one  througli  a  continuous  transformation. 

'The  Poincare  group,  also  called  fundamental  group,  is  the  quotient  of  the  set  of  closed  paths  by  the 
homotopy  equivalence  relation.  Each  element  of  this  group  defines  in  some  way  the  number  of  turns  that  a 
path  makes 
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Figure  13:  Structure  of  the  8  DOF  serial  manipulator. 

6.5  Experimental  Results 

We  have  tested  successfully  the  Monte-Carlo  procedure  on  several  different  robots.  First, 
we  applied  it  on  the  3  DOF  bar  as  shown  in  figure  8.  The  computation  time  wais  about  ten 
times  slower  than  with  the  classical  best-first  search  approach,  but  we  estimate  that  par<illel 
implementations  of  the  procedure  will  reduce  dramatically  the  computation  time. 

Then,  we  applied  the  same  procedure  on  a  8  DOF  serial  manipulator  with  only  revolute 
joints.  The  overall  structure  of  this  arm  is  presented  in  figure  13. 

Figures  14  and  15  illustrate  two  different  motions  of  the  robot  in  a  2D  workspace.  The 
paths  were  generated  by  the  planner. 

In  the  examples  shown  in  figures  14  and  15,  we  used  a  heuristic  potential  U  computed 
with  only  one  point  located  at  the  end-effector  of  the  kinematic  chain.  The  algorithm  was 
able  to  find  the  solution  in  2  minutes  for  the  first  example  (forward  motion),  and  30  seconds 
for  the  second  one  (backward  motion). 

\V'e  also  applied  the  procedure  on  a  10  DOF  non-serial  manipulator  arm  including  both 
prismatic  and  revolute  joints.  The  overall  structure  of  this  arm  is  presented  in  figure  16. 

The  10  DOF  manipulator  robot  includes  7  revolute  joints  and  3  prismatic  joints.  For 
this  manipulator,  the  total  number  of  discrete  configurations  is  of  the  order  of  100'°  =  10^° 
configurations. 

Figures  17,  and  IS  illustrate  two  different  motions  of  the  robot  in  a  2D  workspace.  The 
paths  were  generated  by  the  planner. 
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Figure  16:  Structure  of  the  10  DOF  manipulator  robot. 

In  the  examples  shown  in  figures  17  and  18,  we  used  a  heuristic  potential  U  computed 
with  only  two  points  located  at  the  end  points  of  the  two  kinematic  chains.  The  algorithm 
was  able  to  find  the  solution  in  3  minutes  for  the  first  example,  and  2  minutes  for  the  second 
one. 

Finally,  we  applied  the  same  procedure  to  a  simple  example  of  multi-robot  cooperation. 
Figure  19  displays  two  3  DOF  mobile  robots  at  the  two  extremities  of  a  corridor.  Figure  20 
displays  the  path  found  when  the  goal  of  the  small  robot  is  the  upper  right  corner  while  the 
goal  of  the  long  robot  is  the  upper  left  corner.  Finally,  figure  21  displays  the  path  found 
when  the  goal  of  the  two  robots  is  simply  to  exchange  their  respective  locations.  These  last 
examples  where  solved  in  about  30  seconds  of  computation  time. 

All  the  examples  presented  to  the  planner  have  been  solved  within  a  few  minutes  of 
computation.  So  far,  we  did  not  find  any  case  where  the  planner  fails. 

The  next  subsection  discusses  the  completeness  issue  using  the  fundamental  properties 
of  the  Wiener-Levy  process. 

6.6  Probabilistic  Completeness  of  Monte-Carlo  Procedures 

In  this  subsection,  we  discuss  the  completeness  of  the  Monte-Carlo  procedure  described 
above.  Of  course,  procedures  involving  random  processes  can  never  be  deterministically 
complete.  However,  weaker  completeness  results  can  be  obtained  under  particular  circum¬ 
stances.  The  algorithm  above  takes  as  input  a  map  of  the  workspace,  and  a  description  of 
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Figure  17:  Successive  configurations  during  a  forward  motion  (10  DOF  robot). 


Figure  18:  Successive  configurations  during  a  backward  motion  (10  DOF  robot). 
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Figure  19:  The  corridor  problem  for  a  6  DOF  multirobot  system. 

the  initial  and  goal  configurations.  Its  output  is  two-fold.  First  the  algorithm  gives  an  an¬ 
swer  to  the  existential  question:  is  there  a  path  between  the  initial  and  goal  configurations? 
Second,  if  the  path  does  exist,  it  provides  an  explicit  solution  in  configuration  space.  This 
may  appear  surprising:  a  Monte- Carlo  procedure  can  never  give  a  definitive  answer  to  such 
an  existential  question  in  a  finite  amount  of  time!  This  is  why  we  arbitrarily  bounded  the 
number  of  random  motions  generated  from  a  given  local  minimum:  the  algorithm  does  not 
guarantee  to  answer  properly  to  the  existential  question,  but  guarantees  to  answer  it  in  a 
finite  amount  of  time. 

We  claim  that  if  the  number  of  random  motions  performed  from  a  given  local  minimum 
is  not  bounded,  the  algorithm  weakly  converges  towards  the  goal.  This  claim  is  based 
on  a  very  general  property  of  the  Wiener-Levy  process  that  we  describe  now.  Whenever 
the  freespace  is  connected  and  relatively  compact,  and  given  a  Brownian  motion  w{t)  with 
reflective  boundaries  starting  at  any  point  qisiT  in  freespace,  the  probability  for  w  to  reach 
during  [0,  <]  any  given  neighborhood  B  of  the  goal  qcoAL  at  least  once,  converges  towards  1 
when  the  computation  time  converges  towards  infinity.  Formally,  we  have: 

lim  P(Me(t))  =  1  (14) 

*00  '  ' 

where 

Meit)  =  {Bt  E[0,i]  1  xv{t)  q  D) 

If  statement  (14)  is  true,  then  our  Monte-Carlo  procedure  obviously  converges  weakly  to¬ 
wards  the  global  minimum  of  the  potential  field  U.  As  a  matter  of  fact,  we  may  consider  a 
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Successive  configurations  during  a  simple  motion  (6  DOF  multirobot). 
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neighborhood  B  of  the  global  minimum  containing  no  other  local  minimum.  As  we  generate 
arbitrarily  many  and  arbitrarily  long  Brownian  motions,  the  probability  that  the  process 
attains  B  converges  towards  1  by  equation  (14).  But  then  the  process  becomes  a  gradient 
procedure  which  reaches  immediately  the  global  minimum. 

We  now  prove  the  validity  of  statement  14.  It  is  easily  proven  if  w{t)  is  a  1-dimensional 
Wiener-Levy  process  by  using  the  Desire  Andre  reflection  principle  [Papoulis  65].  Its  gener¬ 
alization  to  any  connected  freespace  C/ree  of  any  compact  manifold  C  equipped  with  a  volume 
measure®  p,  is  still  true,  but  based  on  a  deep  result  about  the  asymptotic  behavior  of  u;(i). 

It  is  important  to  notice  that  the  compactness  assumption  for  C  is  crucial.  If  C  is  not 
compact,  the  hypothesis  has  been  proven  to  be  false  for  n  >  2  [Doob  84).  As  a  matter  of 
fact,  if  C/ree  IS  simply  equal  to  R'*,  we  have  the  following  result  [Doob  84): 

Theorem  2  (  Hitting  of  a  ball  B  by  a  Brownian  motion  in  R”  ) 

Let  w  be  a  Brownian  motion  in  R”  and  B  a  non-empty  ball  not  containing  the  starting 
point  of  w. 

Ifn  <  2, 

^lim  P{MB{i))  =  1 

Ifn  >  3, 

^li^  P(MB(t))  <  1 

Now,  let  us  consider  a  connected  open  subset  C/ree  of  a  compact  manifold  with  smooth 
boundaries,  equipped  with  a  volume  measure  p  (In  our  case,  the  measure  p  is  simply  p  ~ 
■^dqi  X  ...  X  It  can  be  shown  ([Schuss  80])  that  the  probability  distribution  of  a 

/i-standard  Brownian  motion  with  reflective  boundaries  starting  at  any  given  point  qisiT  is 
the  solution  of  the  heat  equation^: 

V(9,f)  G  C/ree  X  R+,  ?(?. 0)  =  “  9/N/r) 

with  Neumann  boundary  conditions; 

V(g,0eaC/reeXR+,  =  0 

01/ 

where  u  is  the  normal  vector  to  the  boundary.  It  can  also  be  shown  ([Schuss  SO])  that 
this  time- varying  probability  distribution  converges  towards  a  stationary  distribution  Poo{<i)- 
Therefore,  p^o  is  the  solution  of  the  following  problem: 

V<7  €  C/ree,  V^oo  =  0 

®A  volume  measure  over  a  compact  topological  space  is  a  measure  which  is  finite  and  non-zero  for  any 
non-empty  open  subset 

®[Schuss  80]  only  treats  the  case  of  Euclidean  space,  but  its  generalization  to  manifolds  is  straightforward 
[Kendall  87] 
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V<7  G  ac/ree,  ^  =  0 

and 

/  Poo{q)dniq)  =  1 

But  the  unique  solution  of  this  problem  is  the  /i-uniform  distribution: 

As  the  /i-standaxd  Brownian  motion  is  time-homogeneous,  we  can  apply  recursively  the 
Markov  property  to  infer  (14). 

Finally,  we  can  state: 

Monte-Carlo  procedure  for  robot  motion  planning 

Let  U  be  a  differentiable  real-valued  function  over  an  open  subset  C/ree  of  a  compact 
manifold  C  equipped  with  a  volume  measure  p.  We  assume  that  the  boundary  of  Cjree  *5 
smooth.  We  assume  that  U  possesses  a  unique  global  minimum  qaoAL,  ond  that  the  number 
of  local  minima  of  U  is  finite. 

We  assume  that  from  any  point  q  in  C/ree  laying  in  the  attractor  of  a  given  local  minimum 
qioc  the  gradient  procedure  which  leads  from  q  to  qi^e  can  be  performed  in  a  finite  amount  of 
time  ToRADiq)-  will  denote  this  procedure  by  GRADIENT (q,t). 

We  denote  by  RANDOM(q,t)  the  p-standard  Brownian  motion  process  with  reflective 
boundaries  starting  at  q  &  C/ree- 

We  consider  the  following  process  Q{t)  starting  from  any  configuration  (5(0)  =  qiNlT- 

Compute  <5(0  =  GRADIENT{qif/[T,l)  from  t  =  0 
until  we  reach  a  local  minimiim  qcur  =  QiTcRADiqrtfiT))  ■ 

Set  tcur  =  ToRADiqiNir)  • 

begin  loop 

begin  loop 

Generate  a  raindora  time  Thand  following 
the  distribution  given  in  formula  (13). 

Compute  Qcandidau{t)  =  RANDOM{qcuT,t)  from  <cur 

until  the  time  <aur  for  which  either  of  the  two  conditions 

UiQcandidaUt))  <  (/(w)  or  t  =  t^r  +  TfUND  is  true. 

The  final  point  of  the  random  procedure  is 

denoted  qaux  ~  Q candidate(.l  aux)  • 
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Compute  Qcandidate{t)  =  GRADIENT{qaux,t)  from  taux  tO 
^new  ~  ^aux  'Rg RAoi/Iatix') 

where  another  local  minimum  qnew  =  Q candidate{tnew)  is  reached, 
if  ^(Qcan(<i(^ate(^neu/))  ^  ^ {.Qcur)  then  eXlt 

end  loop 

Set  qcur  ~  Qnev/  Snd  ^cur  “  inew  • 

Concatenate  the  process  Q{t)  computed  so  far  with  Qcandidate{t)  ■ 

end  loop 


The  process  Q  weakly  converges  towards  the  global  minimum  x^in  of  U .  In  other  words, 
for  any  fx-measurable  neighborhood  B  of  x^inf  have: 

^i^p(g(t)  6  P)  =  1 


This  procedure  performs  a  random  search  of  the  graph  of  local  minima  of  the  potential 
field  in  a  best-first  fashion,  using  as  heuristic  the  potential  field  itself.  It  behaves  very 
differently  from  the  simulated  annealing  procedures  [Kirk  83],  [Geman  84],  which  perform  a 
breadth-first  search  of  the  local  minima. 

Basically,  all  simulated  annealing  procedures  can  be  shown  to  behave  similarly  to  the 
following  process: 

Simulated  annealing  procedure  for  optimization 

Let  U  be  a  differentiable  function  over  (0, 1]”  C  R"  with  a  single  global  minimum  Xmm- 
Let  w  be  a  standard  Brownian  motion  (for  the  Lebesgue  measure)  with  reflective  boundaries 
in  [0, 1]”.  Let  T{t)  be  a  non-negative  function  converging  towards  0  when  t  — ►  oo  such  that 
for  some  constant  K  and  for  any  t>to  the  following  inequality  holds: 

K 

~  logt 

Consider  the  solution  X  of  the  following  stochastic  differential  equation^^: 

dX{t)  =  -VU{X{t))dt  4-  y/2T{t)dw{t),  A'(0)  —  UCq  ^  1 0  } 

If  K  is  large  enough,  the  process  X{t)  weakly  converges  towards  Xmin- 

The  proof  of  this  result  can  be  found  in  [Geman  84]. 

‘°Here,  the  differentials  have  to  be  taken  as  ltd  differentials  [Ikeda  81] 
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The  logarithmic  rate  for  the  temperature  is  extremely  slow  in  practical  applications.  Most 
of  the  authors  applying  this  technique  choose  a  much  faster  annealing  schedule  (in  general  an 
exponential  one),  in  which  case  the  convergence  result  collapses.  In  the  case  of  robot  motion 
planning,  experimental  results  show  that  a  classical  annealing  procedure  in  not  practical. 
In  fact,  our  Monte-Carlo  approach  can  be  considered  as  a  particular  case  of  the  diffusion 
process  defined  by  (15)  where  the  temperature  coefficient  T{t)  is  chosen  to  be  0  during  the 
gradient  procedure  and  to  -f-oo  during  the  random  motions.  Instead  of  choosing  a  slowly 
decreasing  annealing  schedule,  we  perform  a  sequence  of  temperature  switches  between  0 
and  -hoo.  The  switches  are  performed  when  a  local  minimum  is  reached. 

The  diffusion  processes  of  the  form  (15)  are  called  Smoluchowski- Kramers  processes,  and 
their  probability  distribution  p{x,t)  is  the  solution  of  a  partial  differential  equation  called 
forward  Kolmogorov  equation^^  (Schuss  80]: 

^  =  L{p),  p(x,  0)  =  ^(x  -  xo) 
where  L  is  the  Fokker-Planck  operator: 

L{p)  =  v.(pvc/)  +  rvV 

The  study  of  the  convergence  rate  of  such  processes  then  reduces  to  the  study  of  the  smallest 
(in  absolute  value)  non-zero  eigenvalue  \i  of  the  Fokker-Planck  operator.  [Matkow  81]  and 
[Schuss  80]  give  approximate  formulas  for  computing  Ai  in  some  simple  cases.  However, 
in  most  cases,  Xi  can  only  be  computed  numerically,  and  the  only  clue  for  estimating  the 
convergence  rate  of  general  diffusion  processes  is  computer  simulation. 

7  Some  Remarks  on  Parallel  Implementations 

Thanks  to  the  distributed  data  representation  chosen,  the  algorithm  above  is  highly  paral- 
lelizable. 

First,  the  hierarchical  decoupled  decomposition  of  the  problem  allows  to  compute  in 
parallel  all  the  differents  levels  of  hierarchy,  instead  of  performing  sequentially  a  coarse-to- 
fine  approach.  This  parallelization  process  is  straightforward,  and  there  is  no  need  to  detail 
it  further. 

Then,  for  each  resolution  level  chosen,  the  planning  technique  can  be  roughly  divided  into 
three  decoupled  steps.  The  first  step  consists  of  computing  in  workspace  a  few  numerical 
potentials  (the  distance  potential,  and  the  attractive  potential  fields  without  local  minima) 
using  the  wavefront  expansion  technique  described  in  section  3.  The  second  step  consists 
of  performing  an  incremental  best-first  search  of  the  connectivity  graph  built  from  the  set 
of  local  minima  of  the  overall  configuration  space  potential  field  U.  The  third  step  consists 
of  smoothing  the  path  obtained  by  solving  a  classical  two  points  boundary  value  problem. 

"[f  U  =  0  and  T  =  1/2  this  reduces  to  the  heat  equation 


47 


We  describe  respectively  in  the  three  subsections  below  the  parallelization  issues  relative  to 
these  three  decoupled  phases. 

7.1  Wavefront  Expansion  in  Workspace 

The  basic  idea  underlying  all  workspace  computations  in  our  model  is  to  perform  a  wavefront 
expansion  of  the  information.  In  the  case  of  the  distance  to  the  obstacles  dj,  the  wavefront 
is  initialized  to  0  at  the  obstacles  boundaries.  In  the  case  of  the  attractive  potential  fields, 
the  wavefront  is  initialized  to  0  at  the  goal  point.  Then,  at  each  iteration  of  the  expansion, 
the  current  wavefront  oldfront  is  transformed  in  the  expanded  front  newfront. 

There  are  two  basic  ways  for  parallelizing  a  wavefront  expansion.  The  first,  and  theoreti¬ 
cally  the  most  efficient  way,  is  to  allocate  one  virtual  processing  unit  to  each  of  the  members 
of  the  front.  But  two  members  of  the  current  front  oldfront  may  interfere  when  they  are 
expanded  because  they  may  produce  identical  members  of  the  next  front  newfront,  and  in 
fact  they  do  interfere  systematically  for  any  non-planar  geometrical  wavefront.  Therefore, 
the  processing  units  need  to  share  memory  information.  It  is  well-known  that  shared-memory 
parallelism  cannot  be  performed  efficiently  in  a  large  scale,  as  it  is  necessary  for  our  problem. 
Therefore,  the  only  solution  remaining  is  the  so-called  “data-parallelism” ,  which  consists  of 
affecting  to  each  point  of  the  workspace  bitmap  a  virtual  processing  unit.  In  this  process, 
each  virtual  processor  will  have  its  own  memory  and  will  only  need  to  communicate  with  its 
geometrical  neighbors.  Parallel  architectures  such  as  to  one  used  in  the  Connection  Machine 
(Hillis  85]  are  particularly  well-suited  for  this  type  of  application.  However,  at  each  cycle 
time,  only  those  processors  which  happen  to  be  part  of  the  current  wavefronts  oldfront 
and  newfront  are  actually  active.  Therefore,  this  brute  force  parallelism  wastes  a  lot  of 
parallelism  power.  We  believe  nevertheless  that  this  approach  is  the  only  realistic  way  to 
envision  parallelism  for  geometrical  problems. 

More  precisely,  let  us  consider  an  SIMD  machine  with  distributed  memory  having  a 
capability  of  local  communication  with  2D  or  3D  geometrical  neighbors.  This  is  typically  the 
case  for  the  Connection  Machine  [Hillis  85).  We  suppose  that  this  machine  has  NBPIX  = 
2*®  =  65,536  processors. 

We  consider  a  workspace  of  dimension  n  =  2  or  3  at  resolution  r  =  —  logj  S  =  S, 
each  coordinate  having  therefore  DIM  =  1/6  =  256  discrete  values.  The  total  number  of 
workspace  bitmap  points  is  DIM”',  that  is  65,536  in  2D  and  16,777,216  in  3D.  Therefore, 
the  number  RATIO  of  virtual  processors  (i.e.  of  bitmap  points)  that  we  have  to  associate 
to  each  physical  processor  is: 

DIM” 

RATIO  = 

This  ratio  is  1  in  2D  and  256  in  3D. 

The  active  wavefront  at  each  time  of  the  expansion  is  an  (n  —  l)-dimensional  subset  of  the 
n-dimensional  workspace.  For  a  typical  workspace  with  a  few  obstacles,  the  total  number  of 
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points  in  this  active  wavefront  is  COEF  x  =  4  x  In  2D,  this  amounts 

to  1024.  In  3D,  it  is  262,144. 

Let  us  denote  by  T,  is  the  computation  time  that  would  be  required  to  perform  the 
expansion  using  a  single  serial  processor.  We  aissume  that  the  number  of  physical  processors 
is  always  less  than  the  number  of  virtual  processors,  that  is  RAT  10  =  DIM^/N BPIX.  The 
parallel  computation  time  Tp  is  approximately  (neglecting  the  communication  time  between 
processors,  which  is  realistic  in  this  case): 

^  Ts  DIM 


T„  =  RATIO  X 


COEF  X  DIM" 


DIM 

COEF  X  NBPIX 


The  above  formula  leads  to  two  very  important  conclusions: 

1)  As  long  as  the  number  of  physical  processors  is  less  than  the  number  of  virtual  processors, 

the  parallelism  speedup  of  a  wavefront  expansion  does  not  depend  on  the  dimension  of 
the  workspace. 

2)  To  obtain  an  actually  significant  speedup  with  distributed  memory  data- parallelism,  the 

number  of  physical  processors  has  to  be  much  larger  thaui  the  number  of  discrete  points 
representing  each  coordinate  of  the  workspace. 

We  obtain  the  same  speedup  for  a  2D  or  a  3D  workspace: 

r  -  JL 

”  1024 


If  the  power  of  each  processor  in  the  paradlel  machine  is  100  times  smaller  than  the  power 
of  classical  workstations  (this  is  approximately  the  case  for  current  data-parallel  machines), 
we  finally  get  a  total  speedup  of  one  order  of  magnitude.  As  we  are  able  to  perform  a  2D 
wavefront  expansion  on  a  workstation  in  about  1/4  seconds,  the  same  computation  could 
theoretically  be  performed  in  about  1/40  seconds  on  a  data-parallel  machine.  For  a  3D 
expansion  of  the  same  size,  we  will  need  256  times  more  time,  that  is  approximately  1 
minute  on  a  worksation  and  6  seconds  on  a  data-parallel  machine. 

Finally,  we  can  conclude  that  a  speedup  of  more  than  one  order  of  magnitude  can  only 
be  made  on  a  general  purpose  SIMD  computer  only  if  considerable  improvements  are  made 
in  the  hardware. 

Therefore,  we  believe  that  a  more  realistic  solution  for  workspace  computations  is  to 
design  a  dedicated  chip  for  geometrical  wavefront  expansion.  As  the  performances  of  such 
dedicated  systems  highly  depend  on  several  technological  choices,  it  is  not  yet  possible  to  give 
an  accurate  estimate  of  the  final  speedup.  We  think  that  real-time  capabilities  (i.e.  a  fraction 
of  a  second)’  are  achievable  for  3D  workspaces  with  current  microprocessor  technology. 


7.2  Parallelizing  the  Search  in  Configuration  Space 

The  exit  procedure  used  to  escape  from  a  given  local  minimum  qioc  consists  of  generating 
several  random  motions  from  the  local  minimum.  All  these  random  motions  can  be  performed 
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in  parallel;  We  may  affect  to  each  of  them  a  different  processor,  all  of  these  working  with  a 
shared  memory.  The  processor  which  will  be  the  first  to  find  a  new  local  minimum  better 
than  qtoc  will  stop  all  the  other  processors.  This  parallelization  is  straightforward,  and  there 
is  no  need  to  detail  it  further.  For  complex  manipulator  arms  like  the  one  described  in  section 
6.5,  the  number  of  random  motions  necessary  the  exit  from  a  local  minimum  can  be  of  several 
tens  for  some  difficult  examples.  As  the  computation  time  required  for  the  gradient  motions 
is  neglectable  compared  to  the  time  required  for  exiting  a  local  minimum  on  a  sequential 
machine,  the  speedup  obtained  through  this  parallelization  may  be  of  several  tens.  For 
example,  in  the  setting  shown  in  figure  17,  the  total  computation  time  could  decrease  from 
a  few  minutes  to  a  few  seconds  by  parallelizing  the  random  motions  generation. 

During  this  search  in  configuration  space,  the  potential  field  U  eis  well  as  the  distance 
Di  of  the  robot  to  the  obstacles  have  to  be  computed  for  each  configuration  explored.  The 
computation  of  D\  involves  all  the  points  of  the  robot  body,  without  interaction  between 
the  different  points.  This  is  yet  another  opportunity  for  parallelism.  As  a  matter  of  fact, 
we  may  affect  to  each  point  p  on  the  robot  body  a  dedicated  processor  computing  the 
forward  kinematics  q  — ►  X(p,q)  of  this  point  in  the  workspace,  and  communicating  with 
the  workspace  bitmap  processing  unit  to  output  the  values  of  the  workspace  potential  field 
and  the  distance  to  the  obstacles  di.  Then,  the  results  of  all  these  parallel  computations 
have  to  be  joined  together  (through  computations  of  minima  and  maxima)  in  order  to  obtain 
the  final  overall  potential  U  and  the  distance  Dj.  Parallel  computers  are  very  efficient  for 
computing  non-linear  functions  such  as  the  maximum  or  minimum  of  the  output  of  all 
the  processors.  For  complex  manipulators  arms  operating  nearby  obstacles  as  it  is  the 
case  for  the  setting  shown  in  figure  17,  the  resulting  speedup  could  be  of  two  orders  of 
magnitude  (we  need  about  100  control  points  when  the  arm  is  close  to  the  obstacles).  As  this 
last  parallelization  can  be  combined  multiplicatively  with  the  parallelization  of  the  random 
motions  generation,  the  total  speedup  achievable  during  the  configuration  space  search  is  of 
3  to  4  orders  of  magnitude.  In  the  example  shown  in  figure  17,  the  total  computation  time 
could  fall  from  3  minutes  to  1/40  seconds  with  processors  equivalent  to  those  available  on 
current  workstations.  Of  course,  this  impressive  speedup  would  require  a  very  high  number 
of  processors  (about  10000)  of  the  same  power  than  a  current  workstation.  But  even  if  only 
100  processors  are  available,  it  is  still  thinkable  to  distribute  10000  virtual  units  over  100 
physical  processors,  so  that  the  speedup  would  still  be  of  two  orders  of  magnitude. 

7.3  Concurrent  Computation  of  Two-Points  Boundary  Value  Prob¬ 
lems 

Smoothing  the  path  obtained  from  the  Monte-Carlo  procedure  is  a  classical  quadratic  two- 
points  boundary  value  problem.  On  a  parallel  computer,  it  is  possible  to  affect  one  vir¬ 
tual  processor  to  each  discrete  point  on  the  path.  As  we  only  consider  first  derivatives  in 
our  objective  functional,  each  processor  only  needs  to  communicate  with  its  two  neighbors; 
its  predecessor  and  its  successor.  Many  algorithms  are  available  to  optimize  in  parallel  a 
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quadratic  functional.  The  simplest  possible  is  the  Jacobi  algorithm  (i.e.  steepest  descent 
gradient  applied  to  a  quadratic  function).  Fancier  algorithms  like  preconditioned  conjugate 
gradient  can  also  be  implemented  efficiently  on  a  data-parallel  machine.  In  all  cases,  recent 
experiments  [SRI  89]  show  that  impressive  speedups  are  obtained  through  the  parallelization 
of  such  algorithms. 

In  conclusion,  we  believe  that  a  dramatic  speedup  can  be  achieved  in  the  near  future 
for  this  Robot  Motion  Planning  algorithm,  either  by  implementing  it  on  a  large  scale  data- 
parallel  machine,  or  by  designing  a  dedicated  parallel  architecture.  This  would  lead  to 
real-time  performances  for  robots  with  a  small  number  of  DOF,  and  to  time  delays  of  a  few 
seconds  only  for  complex  mcinipulators  with  many  DOF. 

8  Conclusion 

In  this  paper,  we  have  presented  a  distributed  representation  approach  to  path  planning  ap¬ 
plicable  for  any  problem  in  open  freespace.  The  same  approach  can  be  used  for  mobile  robots 
(with/without  non-holonomic  constraints)  and  for  arbitrary-shaped  manipulator  arms. 

The  approach  essentially  consists  of  discretizing  both  the  workspace  and  the  configuration 
space  of  the  robot,  and  performing  a  best-first  search  of  a  path  using  powerful  numerical 
potential  fields. 

The  approach  has  solved  3  DOF  robot  problems  solved  by  previous  planners,  but  several 
orders  of  magnitude  faster  (about  one  second,  against  severed  minutes  or  several  tens  of 
minutes  for  previous  planners).  More  importantly,  it  heis  solved  problems  that  fedl  far  outside 
the  range  of  possibilities  of  any  other  existing  planners,  e.g.  3  DOF  robot  problems  with  non- 
holonomic  constraints,  10  DOF  manipulator  robot  problems,  and  multi-robot  cooperation 
problems. 

The  algorithm  is  highly  parallelizable.  We  envision  a  VLSI  implementation  which  should 
allow  to  perform  real-time  path  planning.  This  opens  new  perspectives  on  some  key  issues 
in  robotics  related  to  the  interaction  of  planning  and  execution  in  partially  known  and 
dynamically  changing  environments. 

Current  work  is  conducted  along  several  directions.  First,  we  plan  to  implement  the 
approach  on  robots  operating  in  3D  workspaces.  Second,  we  intend  to  experiment  our 
planner  on  robots  having  several  tens  of  DOF  in  order  to  understand  clearly  its  eventual 
limitations.  Third,  we  plan  to  extend  this  approach  to  motion  planning  in  partially  known 
environment.  Fourth,  we  will  implement  this  approach  on  real  robots  in  order  to  study 
the  new  capabilities  induced  by  very  fast  motion  planning.  Fifth,  we  intend  to  investigate 
possible  generalizations  of  the  distributed  representation  approach  to  motion  planning  and 
control  with  uncertainty  in  control  and  sensing  for  part  mating  and  dextrous  manipulation 
operations.  As  a  matter  of  fact,  we  believe  that  this  distributed  representation  of  the  world 
can  be  extended  in  order  to  include  not  only  positions,  but  also  velocities,  accelerations, 
internal  and  external  forces,  and  uncertainty. 
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